<!DOCTYPE html>
<html class="writer-html5" lang="en" >
<head>
  <meta charset="utf-8" />
  <meta name="viewport" content="width=device-width, initial-scale=1.0" />
  <title>bosdyn.client.math_helpers &mdash; Spot 5.0.1.1 documentation</title>
      <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
      <link rel="stylesheet" href="../../../_static/style.css" type="text/css" />
    <link rel="shortcut icon" href="../../../_static/bd-favicon.png"/>
    <link rel="canonical" href="https://dev.bostondynamics.com/_modules/bosdyn/client/math_helpers.html" />
  <!--[if lt IE 9]>
    <script src="../../../_static/js/html5shiv.min.js"></script>
  <![endif]-->
  
        <script src="../../../_static/jquery.js"></script>
        <script src="../../../_static/_sphinx_javascript_frameworks_compat.js"></script>
        <script data-url_root="../../../" id="documentation_options" src="../../../_static/documentation_options.js"></script>
        <script src="../../../_static/doctools.js"></script>
        <script src="../../../_static/sphinx_highlight.js"></script>
    <script src="../../../_static/js/theme.js"></script>
    <link rel="index" title="Index" href="../../../genindex.html" />
    <link rel="search" title="Search" href="../../../search.html" /> 
</head>

<body class="wy-body-for-nav"> 
  <div class="wy-grid-for-nav">
    <nav data-toggle="wy-nav-shift" class="wy-nav-side">
      <div class="wy-side-scroll">
        <div class="wy-side-nav-search" >

          
          
          <a href="../../../README.html">
            
              <img src="../../../_static/bd-official-white.png" class="logo" alt="Logo"/>
          </a>
              <div class="version">
                5.0.1.1
              </div>
<div role="search">
  <form id="rtd-search-form" class="wy-form" action="../../../search.html" method="get">
    <input type="text" name="q" placeholder="Search docs" aria-label="Search docs" />
    <input type="hidden" name="check_keywords" value="yes" />
    <input type="hidden" name="area" value="default" />
  </form>
</div>
        </div><div class="wy-menu wy-menu-vertical" data-spy="affix" role="navigation" aria-label="Navigation menu">
              <ul>
<li class="toctree-l1"><a class="reference internal" href="../../../docs/concepts/README.html">Concepts</a><ul>
<li class="toctree-l2"><a class="reference internal" href="../../../docs/concepts/about_spot.html">About Spot</a></li>
<li class="toctree-l2"><a class="reference internal" href="../../../docs/concepts/orbit/about_orbit.html">About Orbit (formerly Scout)</a></li>
<li class="toctree-l2"><a class="reference internal" href="../../../docs/concepts/orbit/orbit_api.html">Orbit API</a></li>
<li class="toctree-l2"><a class="reference internal" href="../../../docs/concepts/networking.html">Networking</a></li>
<li class="toctree-l2"><a class="reference internal" href="../../../docs/concepts/base_services.html">Base services</a></li>
<li class="toctree-l2"><a class="reference internal" href="../../../docs/concepts/geometry_and_frames.html">Geometry and Frames</a></li>
<li class="toctree-l2"><a class="reference internal" href="../../../docs/concepts/robot_services.html">Robot services</a></li>
<li class="toctree-l2"><a class="reference internal" href="../../../docs/concepts/estop_service.html">E-Stop</a></li>
<li class="toctree-l2"><a class="reference internal" href="../../../docs/concepts/keepalive_service.html">KeepAlive (BETA)</a></li>
<li class="toctree-l2"><a class="reference internal" href="../../../docs/concepts/audio_visual.html">Audio Visual</a></li>
<li class="toctree-l2"><a class="reference internal" href="../../../docs/concepts/lease_service.html">Lease</a></li>
<li class="toctree-l2"><a class="reference internal" href="../../../docs/concepts/developing_api_services.html">Developing API Services</a></li>
<li class="toctree-l2"><a class="reference internal" href="../../../docs/concepts/service_customization.html">Service Customization</a></li>
<li class="toctree-l2"><a class="reference internal" href="../../../docs/concepts/faults.html">Faults</a></li>
<li class="toctree-l2"><a class="reference internal" href="../../../docs/concepts/autonomy/README.html">Autonomy services</a><ul>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/concepts/autonomy/graphnav_tech_summary.html">Autonomy Technical Summary</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/concepts/autonomy/autonomous_navigation_code_examples.html">Autonomous navigation code examples</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/concepts/autonomy/components_of_autonomous_navigation.html">Components of autonomous navigation</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/concepts/autonomy/docking.html">Docking</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/concepts/autonomy/typical_autonomous_navigation_use_case.html">Typical autonomous navigation use case</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/concepts/autonomy/autonomous_navigation_services.html">Autonomous navigation services</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/concepts/autonomy/graphnav_service.html">GraphNav service</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/concepts/autonomy/graphnav_map_structure.html">GraphNav map structure</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/concepts/autonomy/graphnav_area_callbacks.html">GraphNav area callbacks</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/concepts/autonomy/initialization.html">Initialization</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/concepts/autonomy/localization.html">Localization</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/concepts/autonomy/graphnav_and_robot_locomotion.html">GraphNav and robot locomotion</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/concepts/autonomy/missions_service.html">Missions service</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/concepts/autonomy/autowalk_service.html">Autowalk service</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/concepts/network_compute_bridge.html">Network compute bridge</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/concepts/autonomy/auto_return.html">AutoReturn service</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/concepts/autonomy/directed_exploration.html">Directed Exploration</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/concepts/autonomy/gps.html">GPS</a></li>
</ul>
</li>
<li class="toctree-l2"><a class="reference internal" href="../../../docs/concepts/choreography/README.html">Choreography</a><ul>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/concepts/choreography/choreography_service.html">Choreography Service</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/concepts/choreography/move_reference.html">Move Reference Guide</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/concepts/choreography/custom_gait.html">CustomGait Reference</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/concepts/choreography/choreographer_setup.html">Choreographer Setup</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/concepts/choreography/choreographer.html">Choreographer Overview</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/concepts/choreography/robot_controls_in_choreographer.html">Robot Connections in Choreographer</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/concepts/choreography/animations_in_choreographer.html">Animations in Choreography</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/concepts/choreography/animation_file_specification.html">Animation File Format</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/concepts/choreography/choreography_in_tablet.html">Tablet Choreography Mode</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/concepts/choreography/choreography_in_autowalk.html">Choreography Actions in Autowalk</a></li>
</ul>
</li>
<li class="toctree-l2"><a class="reference internal" href="../../../docs/concepts/joint_control/README.html">Joint Control API</a><ul>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/concepts/joint_control/supplemental_data.html">Supplemental Robot Information</a><ul>
<li class="toctree-l4"><a class="reference internal" href="../../../docs/concepts/joint_control/knee_torque_limits.html">Knee Torque Limits</a></li>
</ul>
</li>
</ul>
</li>
<li class="toctree-l2"><a class="reference internal" href="../../../docs/concepts/arm/README.html">Spot Arm</a><ul>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/concepts/arm/arm_specification.html">Arm and Gripper Specification</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/concepts/arm/arm_concepts.html">Concepts</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/concepts/arm/arm_services.html">Services</a></li>
</ul>
</li>
<li class="toctree-l2"><a class="reference internal" href="../../../docs/concepts/data.html">Spot Data</a><ul>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/concepts/data_acquisition_overview.html">Data Acquisition Overview</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/concepts/data_acquisition_output.html">Data Acquisition Output</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/concepts/writing_services_for_data_acquisition.html">Integrate Payloads with the API</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/concepts/data_buffer_overview.html">Data Buffer Overview</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/concepts/bddf.html">BDDF File Format</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/concepts/data_acquisition_thermal_raw.html">Thermal Raw Data Format</a></li>
</ul>
</li>
</ul>
</li>
<li class="toctree-l1"><a class="reference internal" href="../../../docs/python/README.html">Python</a><ul>
<li class="toctree-l2"><a class="reference internal" href="../../../docs/python/quickstart.html">Quickstart</a></li>
<li class="toctree-l2"><a class="reference internal" href="../../../docs/python/understanding_spot_programming.html">Understanding Spot Programming</a></li>
<li class="toctree-l2"><a class="reference internal" href="../../../python/examples/README.html">Examples</a><ul>
<li class="toctree-l3"><a class="reference internal" href="../../../python/examples/docs/basic_service_examples.html">Basic Service Examples</a><ul>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/hello_spot/README.html">Hello Spot</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/directory/README.html">Directory</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/get_robot_state/README.html">Get Robot State</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/get_robot_state_async/README.html">Get Robot State Async</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/get_image/README.html">Get Image</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/get_world_objects/README.html">Get World Objects</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/get_mission_state/README.html">Get Mission State</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/estop/README.html">E-Stop</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/time_sync/README.html">Time Sync</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/comms_test/README.html">Comms Test</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/disable_ir_emission/README.html">IR Enable/Disable</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/reset_safety_stop/README.html">Reset Safety Stop</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/audio_visual/audio_visual_params/README.html">Audio Visual Params</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/audio_visual/audio_visual_behaviors/README.html">Audio Visual Behaviors</a></li>
</ul>
</li>
<li class="toctree-l3"><a class="reference internal" href="../../../python/examples/docs/robot_behavior_examples.html">Robot Behavior and Commands Examples</a><ul>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/stance/README.html">Stance</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/frame_trajectory_command/README.html">Frame Trajectory</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/spot_light/README.html">Spot Light</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/upload_choreographed_sequence/README.html">Upload Choreographed Sequence</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/xbox_controller/README.html">Xbox Controller</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/wasd/README.html">WASD</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/docking/README.html">Docking</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/animation_recorder/README.html">Animation Recorder</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/auto_return/README.html">Auto Return</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/fan_command/README.html">Fan Commands</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/arm_wasd/README.html">ARM WASD</a></li>
</ul>
</li>
<li class="toctree-l3"><a class="reference internal" href="../../../python/examples/docs/arm_examples.html">Arm Command Examples</a><ul>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/arm_simple/README.html">Simple Arm Motion</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/arm_stow_unstow/README.html">Stow/unstow Arm</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/arm_freeze/README.html">Arm Freeze</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/arm_and_mobility_command/README.html">Arm and Mobility Command</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/arm_with_body_follow/README.html">Arm Command with Body Following</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/arm_constrained_manipulation/README.html">Arm Constrained Manipulation</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/arm_trajectory/README.html">Arm Trajectory</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/arm_trajectory/README.html#long-trajectory">Long Trajectory</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/arm_joint_move/README.html">Arm Joint Move Command</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/arm_force_control/README.html">Arm Force Control Command</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/arm_grasp/README.html">Arm Grasp Command</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/arm_grasp_carry_overrides/README.html">Arm Grasp and Carry Overrides</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/arm_gaze/README.html">Arm Gaze Command</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/arm_surface_contact/README.html">Arm Command with Surface Contact</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/arm_door/README.html">Arm Door Opening Command</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/arm_walk_to_object/README.html">Walk to And Pick Up Object</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/arm_gcode/README.html">Writing Gcode</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/gripper_camera_params/README.html">Gripper Camera Parameters</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/arm_impedance_control/README.html">Arm Impedance Control</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/inverse_kinematics/README.html">Inverse Kinematics</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/arm_wasd/README.html">Arm WASD</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/joint_control/README.html">Wiggle Arm</a></li>
</ul>
</li>
<li class="toctree-l3"><a class="reference internal" href="../../../python/examples/docs/payloads_examples.html">Payloads and Registration Examples</a><ul>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/payloads/README.html">Payloads</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/self_registration/README.html">Self Registration</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/service_faults/README.html">Faults</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/velodyne_client/README.html">Velodyne</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/core_io_gpio/README.html">CORE I/O GPIO</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/metrics_over_coreio/README.html">Overview</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/metrics_over_coreio/README.html#usage">Usage</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/metrics_over_coreio/README.html#components">Components</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/metrics_over_coreio/README.html#recommended-debugging">Recommended debugging</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/extensions/README.html">Extensions</a></li>
</ul>
</li>
<li class="toctree-l3"><a class="reference internal" href="../../../python/examples/docs/perception_world_objects_examples.html">Perception and World Objects Examples</a><ul>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/get_image/README.html">Get Image</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/get_world_objects/README.html">Get World Objects</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/world_object_with_image_coordinates/README.html">World Object With Image Coordinates</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/world_object_mutations/README.html">World Object Mutations</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/visualizer/README.html">Visualizer</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/gripper_camera_params/README.html">Gripper Camera Parameters</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/spot_cam/README.html">Spot CAM Services</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/spot_cam/README.html#spot-cam-video-core-io-extension-example">Spot Cam Video Core IO Extension Example</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/stitch_front_images/README.html">Stitch Front Images</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/get_depth_plus_visual_image/README.html">Project Depth Data on Visual Images</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/service_customization/custom_parameter_image_server/README.html">Custom Parameter Image Server</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/fiducial_follow/README.html">Fiducial Follow</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/spot_tensorflow_detector/README.html">Tensorflow Detector</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/service_customization/custom_parameter_ncb_worker/README.html">Custom Parameter Tensorflow Detector</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/network_compute_bridge/README.html">Machine Learning with the Network Compute Bridge</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/network_compute_bridge/fire_extinguisher_server/README.html">Fire Extinguisher Detector with the Network Compute Bridge</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/ray_cast/README.html">Ray Cast</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/user_nogo_regions/README.html">No-Go Regions</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/gps_service/README.html">GPS</a></li>
</ul>
</li>
<li class="toctree-l3"><a class="reference internal" href="../../../python/examples/docs/logging_examples.html">Logging Examples</a><ul>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/bddf_download/README.html">BDDF Download</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/data_buffer/README.html">Data Buffer</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/data_service/README.html">Data Service</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/logging/README.html">Logging</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/log_status/README.html">Log Status</a></li>
</ul>
</li>
<li class="toctree-l3"><a class="reference internal" href="../../../python/examples/docs/data_acquisition_examples.html">Data Acquisition Examples</a><ul>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/data_acquisition_service/README.html">Data Acquisition Service</a><ul>
<li class="toctree-l5"><a class="reference internal" href="../../../python/examples/data_acquisition_service/signals_coreio_modem_plugin/README.html">Modem Signals</a></li>
</ul>
</li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/service_faults/README.html">Faults</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/service_customization/custom_parameter_image_server/README.html">Custom Parameter Image Service</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/service_customization/custom_parameters_data_acquisition/README.html">Custom Parameter Data Acquisition Plugin</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/get_image/README.html">Test Image Service Implementation with Get Image</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/post_docking_callbacks/README.html">Post Docking Callbacks</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/cloud_upload/README.html">Cloud Upload</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/comms_mapping/README.html">Comms image service</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/comms_mapping/README.html#how-to-use">How to use</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/data_acquisition_service/signals_coreio_modem_plugin/README.html">CoreIO Modem Signals Plugin</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/tester_programs/README.html">Tester Programs</a></li>
</ul>
</li>
<li class="toctree-l3"><a class="reference internal" href="../../../python/examples/docs/autonomy_and_missions_examples.html">Autonomy and Missions Examples</a><ul>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/graph_nav_anchoring_optimization/README.html">Graph Nav Anchoring Optimization</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/graph_nav_command_line/README.html">GraphNav and Recording Service Command Line Interfaces</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/graph_nav_command_line/README.html#example-programs">Example Programs</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/graph_nav_extract_point_cloud/README.html">Graph Nav Extract Point Cloud</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/graph_nav_view_map/README.html">Graph Nav View Map</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/graph_nav_view_gps/README.html">Graph Nav View GPS Data</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/get_mission_state/README.html">Get Mission State</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/remote_mission_service/README.html">Remote Mission Service</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/mission_question_answerer/README.html">Mission Question Answerer</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/mission_recorder/README.html">Mission Recorder</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/replay_mission/README.html">Replay Mission</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/post_docking_callbacks/README.html">Post Docking Callbacks</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/area_callback/README.html">Area Callbacks</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/edit_autowalk/README.html">Edit Autowalk</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/record_autowalk/README.html">Record Autowalk</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/extract_images_from_walk/README.html">Extract Images from Autowalk</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/network_request_callback/README.html">Network Request Callback</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/network_request_callback/README.html#id1">Network Request Callback</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/remote_mission_service/spot_check/README.html">SpotCheck Mission Service</a></li>
</ul>
</li>
<li class="toctree-l3"><a class="reference internal" href="../../../python/examples/docs/joint_control_examples.html">Joint Control API Examples</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../python/examples/docs/orbit.html">Orbit</a><ul>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/orbit/hello_orbit/README.html">Hello Orbit</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/orbit/export_run_archives/README.html">Export Run Archives</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/orbit/export_site_walk_archives/README.html">Export Sitewalk Archives</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/orbit/anomalies/README.html">Anomalies</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/orbit/schedule_mission/README.html">Schedule Mission</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/orbit/runs_response/README.html">Runs Response</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/orbit/send_robot_back_to_dock/README.html">Return to Dock</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/orbit/toggle_mission_based_on_weather/README.html">Mission Toggle</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/orbit/webhook/README.html">Webhook</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/orbit/backups/README.html">Backups</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/orbit/webhook_integration/README.html">Webhook Integrations</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/examples/orbit/webhook_integration/README.html#example-overview">Example Overview</a></li>
</ul>
</li>
</ul>
</li>
<li class="toctree-l2"><a class="reference internal" href="../../../python/README.html">Python Reference Guide</a><ul>
<li class="toctree-l3"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/README.html">Client</a><ul>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/area_callback.html">Area Callback</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/area_callback_region_handler_base.html">Area Callback Region Handler</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/area_callback_service_runner.html">Area Callback Service Runner</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/area_callback_service_servicer.html">Area Callback Servicer</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/area_callback_service_utils.html">Area Callback Service Utils</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/arm_surface_contact.html">Arm Surface Contact</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/async_tasks.html">Async Tasks</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/audio_visual.html">Audio Visual</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/audio_visual_helpers.html">Audio Visual Helpers</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/auth.html">Auth</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/auto_return.html">Auto Return</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/autowalk.html">Autowalk</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/bddf.html">BDDF</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/bddf_download.html">BDDF Download</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/channel.html">Channel</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/command_line.html">Command</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/common.html">Common</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/data_acquisition.html">Data Acquisition</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/data_acquisition_helpers.html">Data Acquisition Helpers</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/data_acquisition_plugin.html">Data Acquisition Plugin</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/data_acquisition_plugin_service.html">Data Acquisition Plugin Service</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/data_acquisition_store.html">Data Acquisition Store</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/data_buffer.html">Data Buffer</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/data_chunk.html">Data Chunk</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/data_service.html">Data Service</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/directory_registration.html">Directory Registration</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/directory.html">Directory</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/docking.html">Docking</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/door.html">Door</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/estop.html">E-Stop</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/error_callback_result.html">Error Callback Result</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/exceptions.html">Exceptions</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/fault.html">Fault</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/frame_helpers.html">Frame Helpers</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/graph_nav.html">Graph Nav</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/gripper_camera_param.html">Gripper Camera Params</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/gps/README.html">GPS</a><ul>
<li class="toctree-l5"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/gps/aggregator_client.html">Aggregator Client</a></li>
<li class="toctree-l5"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/gps/gps_listener.html">GPS Listener</a></li>
<li class="toctree-l5"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/gps/NMEAParser.html">NMEA Parser</a></li>
<li class="toctree-l5"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/gps/ntrip_client.html">NTRIP Client</a></li>
<li class="toctree-l5"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/gps/registration_client.html">Registration Client</a></li>
</ul>
</li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/image.html">Image</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/image_service_helpers.html">Image Service Helpers</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/inverse_kinematics.html">Inverse Kinematics</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/ir_enable_disable.html">IR Enable/Disable</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/keepalive.html">Keep Alive</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/lease.html">Lease</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/lease_resource_hierarchy.html">Lease Resource Hierarchy</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/lease_validator.html">Lease Validator</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/license.html">License</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/local_grid.html">Local Grid</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/log_status.html">Log Status</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html">Math Helpers</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/manipulation_api_client.html">Manipulation API</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/map_processing.html">Map Processing</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/metrics_logging.html">Metrics Logging</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/network_compute_bridge_client.html">Network Compute Bridge</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/payload_registration.html">Payload Registration</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/payload_software_update.html">Payload Software Update</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/payload_software_update_initiation.html">Payload Software Update Initiation</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/payload.html">Payload</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/point_cloud.html">Point Cloud</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/power.html">Power</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/processors.html">Processors</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/ray_cast.html">Ray casting</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/recording.html">Recording</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/robot_command.html">Robot Command</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/robot_id.html">Robot ID</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/robot.html">Robot</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/robot_state.html">Robot State</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/sdk.html">SDK</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/server_util.html">Server Util</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/service_customization_helpers.html">Service Customization Helpers</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/signals_helpers.html">Signals Helpers</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/spot_cam/README.html">Spot CAM</a><ul>
<li class="toctree-l5"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/spot_cam/audio.html">Audio</a></li>
<li class="toctree-l5"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/spot_cam/compositor.html">Compositor</a></li>
<li class="toctree-l5"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/spot_cam/health.html">Health</a></li>
<li class="toctree-l5"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/spot_cam/lighting.html">Lighting</a></li>
<li class="toctree-l5"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/spot_cam/lights_helper.html">Lights Helper</a></li>
<li class="toctree-l5"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/spot_cam/media_log.html">Media Log</a></li>
<li class="toctree-l5"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/spot_cam/network.html">Network</a></li>
<li class="toctree-l5"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/spot_cam/power.html">Power</a></li>
<li class="toctree-l5"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/spot_cam/ptz.html">PTZ</a></li>
<li class="toctree-l5"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/spot_cam/streamquality.html">Stream Quality</a></li>
<li class="toctree-l5"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/spot_cam/version.html">Version</a></li>
</ul>
</li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/spot_check.html">Spot Check</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/time_sync.html">Time Sync</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/token_cache.html">Token Cache</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/token_manager.html">Token Manager</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/units_helpers.html">Units Helpers</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/util.html">Util</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-client/src/bosdyn/client/world_object.html">World Object</a></li>
</ul>
</li>
<li class="toctree-l3"><a class="reference internal" href="../../../python/bosdyn-core/src/bosdyn/README.html">Core</a><ul>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-core/src/bosdyn/bddf/README.html">BDDF</a><ul>
<li class="toctree-l5"><a class="reference internal" href="../../../python/bosdyn-core/src/bosdyn/bddf/base_data_reader.html">Base Data Reader</a></li>
<li class="toctree-l5"><a class="reference internal" href="../../../python/bosdyn-core/src/bosdyn/bddf/block_writer.html">Block Writer</a></li>
<li class="toctree-l5"><a class="reference internal" href="../../../python/bosdyn-core/src/bosdyn/bddf/bosdyn.html">BDDF Conventions</a></li>
<li class="toctree-l5"><a class="reference internal" href="../../../python/bosdyn-core/src/bosdyn/bddf/common.html">Common</a></li>
<li class="toctree-l5"><a class="reference internal" href="../../../python/bosdyn-core/src/bosdyn/bddf/data_reader.html">Data Reader</a></li>
<li class="toctree-l5"><a class="reference internal" href="../../../python/bosdyn-core/src/bosdyn/bddf/data_writer.html">Data Writer</a></li>
<li class="toctree-l5"><a class="reference internal" href="../../../python/bosdyn-core/src/bosdyn/bddf/file_indexer.html">File Indexer</a></li>
<li class="toctree-l5"><a class="reference internal" href="../../../python/bosdyn-core/src/bosdyn/bddf/grpc_proto_reader.html">GRPC Proto Reader</a></li>
<li class="toctree-l5"><a class="reference internal" href="../../../python/bosdyn-core/src/bosdyn/bddf/grpc_reader.html">GRPC Reader</a></li>
<li class="toctree-l5"><a class="reference internal" href="../../../python/bosdyn-core/src/bosdyn/bddf/grpc_service_reader.html">GRPC Service Reader</a></li>
<li class="toctree-l5"><a class="reference internal" href="../../../python/bosdyn-core/src/bosdyn/bddf/grpc_service_writer.html">GRPC Service Writer</a></li>
<li class="toctree-l5"><a class="reference internal" href="../../../python/bosdyn-core/src/bosdyn/bddf/message_reader.html">Message Reader</a></li>
<li class="toctree-l5"><a class="reference internal" href="../../../python/bosdyn-core/src/bosdyn/bddf/pod_series_reader.html">POD Series Reader</a></li>
<li class="toctree-l5"><a class="reference internal" href="../../../python/bosdyn-core/src/bosdyn/bddf/pod_series_writer.html">POD Series Writer</a></li>
<li class="toctree-l5"><a class="reference internal" href="../../../python/bosdyn-core/src/bosdyn/bddf/protobuf_channel_reader.html">Protobuf Channel Reader</a></li>
<li class="toctree-l5"><a class="reference internal" href="../../../python/bosdyn-core/src/bosdyn/bddf/protobuf_reader.html">Protobuf Reader</a></li>
<li class="toctree-l5"><a class="reference internal" href="../../../python/bosdyn-core/src/bosdyn/bddf/protobuf_series_writer.html">Protobuf Series Writer</a></li>
<li class="toctree-l5"><a class="reference internal" href="../../../python/bosdyn-core/src/bosdyn/bddf/stream_data_reader.html">Stream Data Reader</a></li>
</ul>
</li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-core/src/bosdyn/geometry.html">Geometry</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-core/src/bosdyn/util.html">Util</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-core/src/bosdyn/deprecated.html">Deprecated</a></li>
</ul>
</li>
<li class="toctree-l3"><a class="reference internal" href="../../../python/bosdyn-mission/src/bosdyn/mission/README.html">Mission</a><ul>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-mission/src/bosdyn/mission/client.html">Client</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-mission/src/bosdyn/mission/constants.html">Constants</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-mission/src/bosdyn/mission/exceptions.html">Exceptions</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-mission/src/bosdyn/mission/remote_client.html">Remote Client</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-mission/src/bosdyn/mission/server_util.html">Server Util</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-mission/src/bosdyn/mission/util.html">Util</a></li>
</ul>
</li>
<li class="toctree-l3"><a class="reference internal" href="../../../python/bosdyn-choreography-client/src/bosdyn/choreography/client/README.html">Choreography</a><ul>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-choreography-client/src/bosdyn/choreography/client/choreography.html">Choreography</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-choreography-client/src/bosdyn/choreography/client/animation_file_to_proto.html">Animation File to Proto</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-choreography-client/src/bosdyn/choreography/client/animation_file_conversion_helpers.html">Animation File to Proto Helpers</a></li>
</ul>
</li>
<li class="toctree-l3"><a class="reference internal" href="../../../python/bosdyn-orbit/src/bosdyn/orbit/README.html">Orbit (formerly Scout)</a><ul>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-orbit/src/bosdyn/orbit/client.html">Client</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-orbit/src/bosdyn/orbit/utils.html">Utils</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-orbit/src/bosdyn/orbit/exceptions.html">Exceptions</a></li>
</ul>
</li>
<li class="toctree-l3"><a class="reference internal" href="../../../python/bosdyn-scout/src/bosdyn/scout/README.html">Scout (deprecated)</a><ul>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-scout/src/bosdyn/scout/client.html">Client</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-scout/src/bosdyn/scout/utils.html">Utils</a></li>
<li class="toctree-l4"><a class="reference internal" href="../../../python/bosdyn-scout/src/bosdyn/scout/exceptions.html">Exceptions</a></li>
</ul>
</li>
</ul>
</li>
<li class="toctree-l2"><a class="reference internal" href="../../../docs/python/fetch_tutorial/fetch1.html">Fetch Tutorial</a><ul>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/python/fetch_tutorial/fetch2.html">Part 2: Training the Model</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/python/fetch_tutorial/fetch3.html">Part 3: Evaluating the Model</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/python/fetch_tutorial/fetch4.html">Part 4: Autonomous Pick Up</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/python/fetch_tutorial/fetch5.html">Part 5: Detecting People and Playing Fetch</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/python/fetch_tutorial/fetch6.html">Part 6: Running the model on Core IO</a></li>
</ul>
</li>
<li class="toctree-l2"><a class="reference internal" href="../../../docs/python/daq_tutorial/daq1.html">Data Collection Tutorial</a><ul>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/python/daq_tutorial/daq2.html">Part 2: Capturing images</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/python/daq_tutorial/daq3.html">Part 3: Capturing other data</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/python/daq_tutorial/daq4.html">Part 4: Deploying to the CORE I/O</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/python/daq_tutorial/daq5.html">Part 5: Collecting data</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/python/daq_tutorial/daq6.html">Part 6: Processing collected data</a></li>
</ul>
</li>
</ul>
</li>
<li class="toctree-l1"><a class="reference internal" href="../../../docs/payload/README.html">Payloads</a><ul>
<li class="toctree-l2"><a class="reference internal" href="../../../docs/payload/payload_configuration_requirements.html">Payload configuration requirements</a></li>
<li class="toctree-l2"><a class="reference internal" href="../../../docs/payload/mechanical_interfaces.html">Mechanical interfaces</a></li>
<li class="toctree-l2"><a class="reference internal" href="../../../docs/payload/robot_mounting_rails.html">Robot mounting rails</a></li>
<li class="toctree-l2"><a class="reference internal" href="../../../docs/payload/guidelines_for_robust_payload_design.html">Guidelines for robust payload design</a></li>
<li class="toctree-l2"><a class="reference internal" href="../../../docs/payload/robot_electrical_interface.html">Robot electrical interface</a></li>
<li class="toctree-l2"><a class="reference internal" href="../../../docs/payload/coreio_documentation.html">CORE I/O Documentation</a></li>
<li class="toctree-l2"><a class="reference internal" href="../../../docs/payload/coreio_openvpn_extension.html">CORE I/O OpenVPN Extension</a></li>
<li class="toctree-l2"><a class="reference internal" href="../../../docs/payload/configuring_payload_software.html">Configuring payload software</a></li>
<li class="toctree-l2"><a class="reference internal" href="../../../docs/payload/docker_containers.html">Dockerize payload software</a></li>
<li class="toctree-l2"><a class="reference internal" href="../../../docs/payload/spot_core_documentation.html">Pre-3.2 Spot CORE Documentation</a><ul>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/payload/spot_core_portainer.html">Configuring Docker containers in SpotCORE</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/payload/spot_core_cockpit.html">Spot CORE system management tool: Cockpit</a></li>
<li class="toctree-l3"><a class="reference internal" href="../../../docs/payload/spot_core_vnc.html">Spot CORE VNC</a></li>
</ul>
</li>
</ul>
</li>
<li class="toctree-l1"><a class="reference internal" href="../../../docs/protos/README.html">API Protocol</a><ul>
<li class="toctree-l2"><a class="reference internal" href="../../../docs/protos/style_guide.html">Style Guide</a></li>
<li class="toctree-l2"><a class="reference internal" href="../../../protos/bosdyn/api/README.html">Proto Reference Guide</a><ul>
<li class="toctree-l3"><a class="reference internal" href="../../../protos/bosdyn/api/proto_reference.html">Protos</a></li>
</ul>
</li>
</ul>
</li>
<li class="toctree-l1"><a class="reference internal" href="../../../docs/release_notes.html">Release Notes</a></li>
<li class="toctree-l1"><a class="reference external" href="https://github.com/boston-dynamics/spot-sdk">SDK Repository</a></li>
</ul>

        </div>
      </div>
    </nav>

    <section data-toggle="wy-nav-shift" class="wy-nav-content-wrap"><nav class="wy-nav-top" aria-label="Mobile navigation menu" >
          <i data-toggle="wy-nav-top" class="fa fa-bars"></i>
          <a href="../../../README.html">Spot</a>
      </nav>

      <div class="wy-nav-content">
        <div class="rst-content">
          <div role="navigation" aria-label="Page navigation">
  <ul class="wy-breadcrumbs">
      <li><a href="../../../README.html" class="icon icon-home" aria-label="Home"></a></li>
          <li class="breadcrumb-item"><a href="../../index.html">Module code</a></li>
      <li class="breadcrumb-item active">bosdyn.client.math_helpers</li>
      <li class="wy-breadcrumbs-aside">
      </li>
  </ul>
  <hr/>
</div>
          <div role="main" class="document" itemscope="itemscope" itemtype="http://schema.org/Article">
           <div itemprop="articleBody">
             
  <h1>Source code for bosdyn.client.math_helpers</h1><div class="highlight"><pre>
<span></span><span class="c1"># Copyright (c) 2023 Boston Dynamics, Inc.  All rights reserved.</span>
<span class="c1">#</span>
<span class="c1"># Downloading, reproducing, distributing or otherwise using the SDK Software</span>
<span class="c1"># is subject to the terms and conditions of the Boston Dynamics Software</span>
<span class="c1"># Development Kit License (20191101-BDSDK-SL).</span>

<span class="kn">import</span> <span class="nn">math</span>
<span class="kn">import</span> <span class="nn">numbers</span>

<span class="kn">import</span> <span class="nn">numpy</span>
<span class="kn">from</span> <span class="nn">deprecated.sphinx</span> <span class="kn">import</span> <span class="n">deprecated</span>

<span class="kn">from</span> <span class="nn">bosdyn.api</span> <span class="kn">import</span> <span class="n">geometry_pb2</span>


<div class="viewcode-block" id="recenter_value_mod"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.recenter_value_mod">[docs]</a><span class="k">def</span> <span class="nf">recenter_value_mod</span><span class="p">(</span><span class="n">value</span><span class="p">,</span> <span class="n">center</span><span class="p">,</span> <span class="n">amplitude</span><span class="p">):</span>
    <span class="n">new_value</span> <span class="o">=</span> <span class="p">(</span><span class="n">value</span> <span class="o">-</span> <span class="n">center</span><span class="p">)</span> <span class="o">%</span> <span class="n">amplitude</span> <span class="o">+</span> <span class="n">center</span>
    <span class="k">if</span> <span class="n">new_value</span> <span class="o">&gt;=</span> <span class="p">(</span><span class="n">center</span> <span class="o">+</span> <span class="mf">0.5</span> <span class="o">*</span> <span class="n">amplitude</span><span class="p">):</span>
        <span class="n">new_value</span> <span class="o">-=</span> <span class="n">amplitude</span>
    <span class="k">elif</span> <span class="n">new_value</span> <span class="o">&lt;</span> <span class="p">(</span><span class="n">center</span> <span class="o">-</span> <span class="mf">0.5</span> <span class="o">*</span> <span class="n">amplitude</span><span class="p">):</span>
        <span class="n">new_value</span> <span class="o">+=</span> <span class="n">amplitude</span>

    <span class="k">return</span> <span class="n">new_value</span></div>


<div class="viewcode-block" id="recenter_angle_mod"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.recenter_angle_mod">[docs]</a><span class="k">def</span> <span class="nf">recenter_angle_mod</span><span class="p">(</span><span class="n">theta</span><span class="p">,</span> <span class="n">center</span><span class="p">):</span>
    <span class="k">return</span> <span class="n">recenter_value_mod</span><span class="p">(</span><span class="n">theta</span><span class="p">,</span> <span class="n">center</span><span class="p">,</span> <span class="mi">2</span> <span class="o">*</span> <span class="n">math</span><span class="o">.</span><span class="n">pi</span><span class="p">)</span></div>


<div class="viewcode-block" id="angle_diff"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.angle_diff">[docs]</a><span class="k">def</span> <span class="nf">angle_diff</span><span class="p">(</span><span class="n">a1</span><span class="p">,</span> <span class="n">a2</span><span class="p">):</span>
    <span class="k">return</span> <span class="n">recenter_angle_mod</span><span class="p">(</span><span class="n">a1</span> <span class="o">-</span> <span class="n">a2</span><span class="p">,</span> <span class="mf">0.0</span><span class="p">)</span></div>


<div class="viewcode-block" id="angle_diff_degrees"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.angle_diff_degrees">[docs]</a><span class="k">def</span> <span class="nf">angle_diff_degrees</span><span class="p">(</span><span class="n">a1</span><span class="p">,</span> <span class="n">a2</span><span class="p">):</span>
    <span class="k">return</span> <span class="n">recenter_value_mod</span><span class="p">(</span><span class="n">a1</span> <span class="o">-</span> <span class="n">a2</span><span class="p">,</span> <span class="mf">0.0</span><span class="p">,</span> <span class="mf">360.0</span><span class="p">)</span></div>


<div class="viewcode-block" id="Vec2"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.Vec2">[docs]</a><span class="k">class</span> <span class="nc">Vec2</span><span class="p">(</span><span class="nb">object</span><span class="p">):</span>
<span class="w">    </span><span class="sd">&quot;&quot;&quot;Class representing a two-dimensional vector.&quot;&quot;&quot;</span>

    <span class="k">def</span> <span class="fm">__init__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">):</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">=</span> <span class="n">x</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">=</span> <span class="n">y</span>

    <span class="k">def</span> <span class="fm">__str__</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
        <span class="k">return</span> <span class="s1">&#39;X: </span><span class="si">%0.3f</span><span class="s1"> Y: </span><span class="si">%0.3f</span><span class="s1">&#39;</span> <span class="o">%</span> <span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span><span class="p">)</span>

    <span class="k">def</span> <span class="fm">__neg__</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
        <span class="k">return</span> <span class="n">Vec2</span><span class="p">(</span><span class="o">-</span><span class="bp">self</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="o">-</span><span class="bp">self</span><span class="o">.</span><span class="n">y</span><span class="p">)</span>

    <span class="k">def</span> <span class="fm">__mul__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">other</span><span class="p">):</span>
        <span class="k">if</span> <span class="ow">not</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">other</span><span class="p">,</span> <span class="n">numbers</span><span class="o">.</span><span class="n">Number</span><span class="p">):</span>
            <span class="k">raise</span> <span class="ne">TypeError</span><span class="p">(</span><span class="s2">&quot;Can&#39;t multiply types </span><span class="si">%s</span><span class="s2"> and </span><span class="si">%s</span><span class="s2">.&quot;</span> <span class="o">%</span> <span class="p">(</span><span class="nb">type</span><span class="p">(</span><span class="bp">self</span><span class="p">),</span> <span class="nb">type</span><span class="p">(</span><span class="n">other</span><span class="p">)))</span>
        <span class="k">return</span> <span class="n">Vec2</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">*</span> <span class="n">other</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">*</span> <span class="n">other</span><span class="p">)</span>

    <span class="k">def</span> <span class="fm">__rmul__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">lhs</span><span class="p">):</span>
        <span class="k">return</span> <span class="bp">self</span> <span class="o">*</span> <span class="n">lhs</span>

    <span class="k">def</span> <span class="fm">__truediv__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">other</span><span class="p">):</span>
        <span class="k">if</span> <span class="ow">not</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">other</span><span class="p">,</span> <span class="n">numbers</span><span class="o">.</span><span class="n">Number</span><span class="p">):</span>
            <span class="k">raise</span> <span class="ne">TypeError</span><span class="p">(</span><span class="s2">&quot;Can&#39;t divide types </span><span class="si">%s</span><span class="s2"> and </span><span class="si">%s</span><span class="s2">.&quot;</span> <span class="o">%</span> <span class="p">(</span><span class="nb">type</span><span class="p">(</span><span class="bp">self</span><span class="p">),</span> <span class="nb">type</span><span class="p">(</span><span class="n">other</span><span class="p">)))</span>
        <span class="k">return</span> <span class="n">Vec2</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">/</span> <span class="n">other</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">/</span> <span class="n">other</span><span class="p">)</span>

    <span class="c1"># Included for python 2 compatibility</span>
    <span class="k">def</span> <span class="nf">__div__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">other</span><span class="p">):</span>
        <span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="fm">__truediv__</span><span class="p">(</span><span class="n">other</span><span class="p">)</span>

    <span class="k">def</span> <span class="fm">__add__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">other</span><span class="p">):</span>
        <span class="k">if</span> <span class="ow">not</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">other</span><span class="p">,</span> <span class="n">Vec2</span><span class="p">):</span>
            <span class="k">raise</span> <span class="ne">TypeError</span><span class="p">(</span><span class="s2">&quot;Can&#39;t add types </span><span class="si">%s</span><span class="s2"> and </span><span class="si">%s</span><span class="s2">.&quot;</span> <span class="o">%</span> <span class="p">(</span><span class="nb">type</span><span class="p">(</span><span class="bp">self</span><span class="p">),</span> <span class="nb">type</span><span class="p">(</span><span class="n">other</span><span class="p">)))</span>
        <span class="k">return</span> <span class="n">Vec2</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">+</span> <span class="n">other</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">+</span> <span class="n">other</span><span class="o">.</span><span class="n">y</span><span class="p">)</span>

    <span class="k">def</span> <span class="fm">__sub__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">other</span><span class="p">):</span>
        <span class="k">return</span> <span class="bp">self</span> <span class="o">+</span> <span class="p">(</span><span class="o">-</span><span class="n">other</span><span class="p">)</span>

    <span class="k">def</span> <span class="fm">__setitem__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">idx</span><span class="p">,</span> <span class="n">data</span><span class="p">):</span>
        <span class="c1"># pylint: disable=no-else-return</span>
        <span class="k">if</span> <span class="n">idx</span> <span class="o">==</span> <span class="mi">0</span><span class="p">:</span>
            <span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">=</span> <span class="n">data</span>
            <span class="k">return</span>
        <span class="k">elif</span> <span class="n">idx</span> <span class="o">==</span> <span class="mi">1</span><span class="p">:</span>
            <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">=</span> <span class="n">data</span>
            <span class="k">return</span>
        <span class="k">raise</span> <span class="ne">IndexError</span><span class="p">(</span><span class="s2">&quot;Invalid index </span><span class="si">{}</span><span class="s2">&quot;</span><span class="o">.</span><span class="n">format</span><span class="p">(</span><span class="n">idx</span><span class="p">))</span>

    <span class="k">def</span> <span class="fm">__getitem__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">idx</span><span class="p">):</span>
        <span class="c1"># pylint: disable=no-else-return</span>
        <span class="k">if</span> <span class="n">idx</span> <span class="o">==</span> <span class="mi">0</span><span class="p">:</span>
            <span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="n">x</span>
        <span class="k">elif</span> <span class="n">idx</span> <span class="o">==</span> <span class="mi">1</span><span class="p">:</span>
            <span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span>
        <span class="k">raise</span> <span class="ne">IndexError</span><span class="p">(</span><span class="s2">&quot;Invalid index </span><span class="si">{}</span><span class="s2">&quot;</span><span class="o">.</span><span class="n">format</span><span class="p">(</span><span class="n">idx</span><span class="p">))</span>

<div class="viewcode-block" id="Vec2.length"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.Vec2.length">[docs]</a>    <span class="k">def</span> <span class="nf">length</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
        <span class="k">return</span> <span class="n">math</span><span class="o">.</span><span class="n">sqrt</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">+</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span><span class="p">)</span></div>

<div class="viewcode-block" id="Vec2.to_proto"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.Vec2.to_proto">[docs]</a>    <span class="k">def</span> <span class="nf">to_proto</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Converts the math_helpers.Vec2 into an output of the protobuf geometry_pb2.Vec2.&quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="n">geometry_pb2</span><span class="o">.</span><span class="n">Vec2</span><span class="p">(</span><span class="n">x</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">y</span><span class="p">)</span></div>

<div class="viewcode-block" id="Vec2.dot"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.Vec2.dot">[docs]</a>    <span class="k">def</span> <span class="nf">dot</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">other</span><span class="p">):</span>
        <span class="k">if</span> <span class="ow">not</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">other</span><span class="p">,</span> <span class="n">Vec2</span><span class="p">):</span>
            <span class="k">raise</span> <span class="ne">TypeError</span><span class="p">(</span><span class="s2">&quot;Can&#39;t dot types </span><span class="si">%s</span><span class="s2"> and </span><span class="si">%s</span><span class="s2">.&quot;</span> <span class="o">%</span> <span class="p">(</span><span class="nb">type</span><span class="p">(</span><span class="bp">self</span><span class="p">),</span> <span class="nb">type</span><span class="p">(</span><span class="n">other</span><span class="p">)))</span>
        <span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">*</span> <span class="n">other</span><span class="o">.</span><span class="n">x</span> <span class="o">+</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">*</span> <span class="n">other</span><span class="o">.</span><span class="n">y</span></div>

<div class="viewcode-block" id="Vec2.cross"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.Vec2.cross">[docs]</a>    <span class="k">def</span> <span class="nf">cross</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">other</span><span class="p">):</span>
        <span class="k">if</span> <span class="ow">not</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">other</span><span class="p">,</span> <span class="n">Vec2</span><span class="p">):</span>
            <span class="k">raise</span> <span class="ne">TypeError</span><span class="p">(</span><span class="s2">&quot;Can&#39;t cross types </span><span class="si">%s</span><span class="s2"> and </span><span class="si">%s</span><span class="s2">.&quot;</span> <span class="o">%</span> <span class="p">(</span><span class="nb">type</span><span class="p">(</span><span class="bp">self</span><span class="p">),</span> <span class="nb">type</span><span class="p">(</span><span class="n">other</span><span class="p">)))</span>
        <span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">*</span> <span class="n">other</span><span class="o">.</span><span class="n">y</span> <span class="o">-</span> <span class="n">other</span><span class="o">.</span><span class="n">x</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span></div>

<div class="viewcode-block" id="Vec2.from_proto"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.Vec2.from_proto">[docs]</a>    <span class="nd">@staticmethod</span>
    <span class="k">def</span> <span class="nf">from_proto</span><span class="p">(</span><span class="n">proto</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Create a math_helpers.Vec2 from a geometry_pb2.Vec2 proto.&quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="n">Vec2</span><span class="p">(</span><span class="n">proto</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="n">proto</span><span class="o">.</span><span class="n">y</span><span class="p">)</span></div></div>


<div class="viewcode-block" id="Vec3"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.Vec3">[docs]</a><span class="k">class</span> <span class="nc">Vec3</span><span class="p">(</span><span class="nb">object</span><span class="p">):</span>
<span class="w">    </span><span class="sd">&quot;&quot;&quot;Class representing a three-dimensional vector.&quot;&quot;&quot;</span>

    <span class="k">def</span> <span class="fm">__init__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="n">z</span><span class="p">):</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">=</span> <span class="n">x</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">=</span> <span class="n">y</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">z</span> <span class="o">=</span> <span class="n">z</span>

    <span class="k">def</span> <span class="fm">__str__</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
        <span class="k">return</span> <span class="s1">&#39;X: </span><span class="si">%0.3f</span><span class="s1"> Y: </span><span class="si">%0.3f</span><span class="s1"> Z: </span><span class="si">%0.3f</span><span class="s1">&#39;</span> <span class="o">%</span> <span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span><span class="p">)</span>

    <span class="k">def</span> <span class="fm">__neg__</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
        <span class="k">return</span> <span class="n">Vec3</span><span class="p">(</span><span class="o">-</span><span class="bp">self</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="o">-</span><span class="bp">self</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="o">-</span><span class="bp">self</span><span class="o">.</span><span class="n">z</span><span class="p">)</span>

    <span class="k">def</span> <span class="fm">__mul__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">other</span><span class="p">):</span>
        <span class="k">if</span> <span class="ow">not</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">other</span><span class="p">,</span> <span class="n">numbers</span><span class="o">.</span><span class="n">Number</span><span class="p">):</span>
            <span class="k">raise</span> <span class="ne">TypeError</span><span class="p">(</span><span class="s2">&quot;Can&#39;t multiply types </span><span class="si">%s</span><span class="s2"> and </span><span class="si">%s</span><span class="s2">.&quot;</span> <span class="o">%</span> <span class="p">(</span><span class="nb">type</span><span class="p">(</span><span class="bp">self</span><span class="p">),</span> <span class="nb">type</span><span class="p">(</span><span class="n">other</span><span class="p">)))</span>
        <span class="k">return</span> <span class="n">Vec3</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">*</span> <span class="n">other</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">*</span> <span class="n">other</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span> <span class="o">*</span> <span class="n">other</span><span class="p">)</span>

    <span class="k">def</span> <span class="fm">__rmul__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">lhs</span><span class="p">):</span>
        <span class="k">return</span> <span class="bp">self</span> <span class="o">*</span> <span class="n">lhs</span>

    <span class="k">def</span> <span class="fm">__truediv__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">other</span><span class="p">):</span>
        <span class="k">if</span> <span class="ow">not</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">other</span><span class="p">,</span> <span class="n">numbers</span><span class="o">.</span><span class="n">Number</span><span class="p">):</span>
            <span class="k">raise</span> <span class="ne">TypeError</span><span class="p">(</span><span class="s2">&quot;Can&#39;t divide types </span><span class="si">%s</span><span class="s2"> and </span><span class="si">%s</span><span class="s2">.&quot;</span> <span class="o">%</span> <span class="p">(</span><span class="nb">type</span><span class="p">(</span><span class="bp">self</span><span class="p">),</span> <span class="nb">type</span><span class="p">(</span><span class="n">other</span><span class="p">)))</span>
        <span class="k">return</span> <span class="n">Vec3</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">/</span> <span class="n">other</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">/</span> <span class="n">other</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span> <span class="o">/</span> <span class="n">other</span><span class="p">)</span>

    <span class="c1"># Included for python 2 compatibility</span>
    <span class="k">def</span> <span class="nf">__div__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">other</span><span class="p">):</span>
        <span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="fm">__truediv__</span><span class="p">(</span><span class="n">other</span><span class="p">)</span>

    <span class="k">def</span> <span class="fm">__add__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">other</span><span class="p">):</span>
        <span class="k">if</span> <span class="ow">not</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">other</span><span class="p">,</span> <span class="n">Vec3</span><span class="p">):</span>
            <span class="k">raise</span> <span class="ne">TypeError</span><span class="p">(</span><span class="s2">&quot;Can&#39;t add types </span><span class="si">%s</span><span class="s2"> and </span><span class="si">%s</span><span class="s2">.&quot;</span> <span class="o">%</span> <span class="p">(</span><span class="nb">type</span><span class="p">(</span><span class="bp">self</span><span class="p">),</span> <span class="nb">type</span><span class="p">(</span><span class="n">other</span><span class="p">)))</span>
        <span class="k">return</span> <span class="n">Vec3</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">+</span> <span class="n">other</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">+</span> <span class="n">other</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span> <span class="o">+</span> <span class="n">other</span><span class="o">.</span><span class="n">z</span><span class="p">)</span>

    <span class="k">def</span> <span class="fm">__sub__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">other</span><span class="p">):</span>
        <span class="k">return</span> <span class="bp">self</span> <span class="o">+</span> <span class="p">(</span><span class="o">-</span><span class="n">other</span><span class="p">)</span>

    <span class="k">def</span> <span class="fm">__setitem__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">idx</span><span class="p">,</span> <span class="n">data</span><span class="p">):</span>
        <span class="c1"># pylint: disable=no-else-return</span>
        <span class="k">if</span> <span class="n">idx</span> <span class="o">==</span> <span class="mi">0</span><span class="p">:</span>
            <span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">=</span> <span class="n">data</span>
            <span class="k">return</span>
        <span class="k">elif</span> <span class="n">idx</span> <span class="o">==</span> <span class="mi">1</span><span class="p">:</span>
            <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">=</span> <span class="n">data</span>
            <span class="k">return</span>
        <span class="k">elif</span> <span class="n">idx</span> <span class="o">==</span> <span class="mi">2</span><span class="p">:</span>
            <span class="bp">self</span><span class="o">.</span><span class="n">z</span> <span class="o">=</span> <span class="n">data</span>
            <span class="k">return</span>
        <span class="k">raise</span> <span class="ne">IndexError</span><span class="p">(</span><span class="s2">&quot;Invalid index </span><span class="si">{}</span><span class="s2">&quot;</span><span class="o">.</span><span class="n">format</span><span class="p">(</span><span class="n">idx</span><span class="p">))</span>

    <span class="k">def</span> <span class="fm">__getitem__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">idx</span><span class="p">):</span>
        <span class="c1"># pylint: disable=no-else-return</span>
        <span class="k">if</span> <span class="n">idx</span> <span class="o">==</span> <span class="mi">0</span><span class="p">:</span>
            <span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="n">x</span>
        <span class="k">elif</span> <span class="n">idx</span> <span class="o">==</span> <span class="mi">1</span><span class="p">:</span>
            <span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span>
        <span class="k">elif</span> <span class="n">idx</span> <span class="o">==</span> <span class="mi">2</span><span class="p">:</span>
            <span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span>
        <span class="k">raise</span> <span class="ne">IndexError</span><span class="p">(</span><span class="s2">&quot;Invalid index </span><span class="si">{}</span><span class="s2">&quot;</span><span class="o">.</span><span class="n">format</span><span class="p">(</span><span class="n">idx</span><span class="p">))</span>

<div class="viewcode-block" id="Vec3.length"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.Vec3.length">[docs]</a>    <span class="k">def</span> <span class="nf">length</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
        <span class="k">return</span> <span class="n">math</span><span class="o">.</span><span class="n">sqrt</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">+</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">+</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span><span class="p">)</span></div>

<div class="viewcode-block" id="Vec3.to_proto"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.Vec3.to_proto">[docs]</a>    <span class="k">def</span> <span class="nf">to_proto</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Converts the math_helpers.Vec3 into an output of the protobuf geometry_pb2.Vec3.&quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="n">geometry_pb2</span><span class="o">.</span><span class="n">Vec3</span><span class="p">(</span><span class="n">x</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="n">z</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">z</span><span class="p">)</span></div>

<div class="viewcode-block" id="Vec3.to_numpy"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.Vec3.to_numpy">[docs]</a>    <span class="k">def</span> <span class="nf">to_numpy</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Converts the math_helpers.Vec3 into an output of numpy format.&quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="n">numpy</span><span class="o">.</span><span class="n">array</span><span class="p">([</span><span class="bp">self</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span><span class="p">])</span></div>

<div class="viewcode-block" id="Vec3.dot"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.Vec3.dot">[docs]</a>    <span class="k">def</span> <span class="nf">dot</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">other</span><span class="p">):</span>
        <span class="k">if</span> <span class="ow">not</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">other</span><span class="p">,</span> <span class="n">Vec3</span><span class="p">):</span>
            <span class="k">raise</span> <span class="ne">TypeError</span><span class="p">(</span><span class="s2">&quot;Can&#39;t dot types </span><span class="si">%s</span><span class="s2"> and </span><span class="si">%s</span><span class="s2">.&quot;</span> <span class="o">%</span> <span class="p">(</span><span class="nb">type</span><span class="p">(</span><span class="bp">self</span><span class="p">),</span> <span class="nb">type</span><span class="p">(</span><span class="n">other</span><span class="p">)))</span>
        <span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">*</span> <span class="n">other</span><span class="o">.</span><span class="n">x</span> <span class="o">+</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">*</span> <span class="n">other</span><span class="o">.</span><span class="n">y</span> <span class="o">+</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span> <span class="o">*</span> <span class="n">other</span><span class="o">.</span><span class="n">z</span></div>

<div class="viewcode-block" id="Vec3.cross"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.Vec3.cross">[docs]</a>    <span class="k">def</span> <span class="nf">cross</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">other</span><span class="p">):</span>
        <span class="k">if</span> <span class="ow">not</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">other</span><span class="p">,</span> <span class="n">Vec3</span><span class="p">):</span>
            <span class="k">raise</span> <span class="ne">TypeError</span><span class="p">(</span><span class="s2">&quot;Can&#39;t cross types </span><span class="si">%s</span><span class="s2"> and </span><span class="si">%s</span><span class="s2">.&quot;</span> <span class="o">%</span> <span class="p">(</span><span class="nb">type</span><span class="p">(</span><span class="bp">self</span><span class="p">),</span> <span class="nb">type</span><span class="p">(</span><span class="n">other</span><span class="p">)))</span>
        <span class="k">return</span> <span class="n">Vec3</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">*</span> <span class="n">other</span><span class="o">.</span><span class="n">z</span> <span class="o">-</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span> <span class="o">*</span> <span class="n">other</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span> <span class="o">*</span> <span class="n">other</span><span class="o">.</span><span class="n">x</span> <span class="o">-</span> <span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">*</span> <span class="n">other</span><span class="o">.</span><span class="n">z</span><span class="p">,</span>
                    <span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">*</span> <span class="n">other</span><span class="o">.</span><span class="n">y</span> <span class="o">-</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">*</span> <span class="n">other</span><span class="o">.</span><span class="n">x</span><span class="p">)</span></div>

<div class="viewcode-block" id="Vec3.from_proto"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.Vec3.from_proto">[docs]</a>    <span class="nd">@staticmethod</span>
    <span class="k">def</span> <span class="nf">from_proto</span><span class="p">(</span><span class="n">proto</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Create a math_helpers.Vec3 from a geometry_pb2.Vec3 proto.&quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="n">Vec3</span><span class="p">(</span><span class="n">proto</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="n">proto</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="n">proto</span><span class="o">.</span><span class="n">z</span><span class="p">)</span></div>

<div class="viewcode-block" id="Vec3.from_numpy"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.Vec3.from_numpy">[docs]</a>    <span class="nd">@staticmethod</span>
    <span class="k">def</span> <span class="nf">from_numpy</span><span class="p">(</span><span class="n">array</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Create a math_helpers.Vec3 from a numpy array.&quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="n">Vec3</span><span class="p">(</span><span class="n">array</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">array</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">array</span><span class="p">[</span><span class="mi">2</span><span class="p">])</span></div></div>


<div class="viewcode-block" id="SE2Pose"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE2Pose">[docs]</a><span class="k">class</span> <span class="nc">SE2Pose</span><span class="p">(</span><span class="nb">object</span><span class="p">):</span>
<span class="w">    </span><span class="sd">&quot;&quot;&quot;Class representing an SE2Pose with position and angle.&quot;&quot;&quot;</span>

    <span class="k">def</span> <span class="fm">__init__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="n">angle</span><span class="p">):</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">=</span> <span class="n">x</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">=</span> <span class="n">y</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">angle</span> <span class="o">=</span> <span class="n">angle</span>

    <span class="k">def</span> <span class="fm">__str__</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
        <span class="k">return</span> <span class="s1">&#39;position -- X: </span><span class="si">%0.3f</span><span class="s1"> Y: </span><span class="si">%0.3f</span><span class="s1"> Yaw: </span><span class="si">%0.1f</span><span class="s1"> deg&#39;</span> <span class="o">%</span> <span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span><span class="p">,</span>
                                                                 <span class="n">math</span><span class="o">.</span><span class="n">degrees</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">angle</span><span class="p">))</span>

<div class="viewcode-block" id="SE2Pose.flatten"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE2Pose.flatten">[docs]</a>    <span class="nd">@staticmethod</span>
    <span class="k">def</span> <span class="nf">flatten</span><span class="p">(</span><span class="n">se3pose</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Flatten a given SE3Pose to an SE2Pose. This will lose height information</span>
<span class="sd">        if the se3pose provided is not gravity aligned. The common gravity aligned</span>
<span class="sd">        frames are odom, vision, and flat_body.</span>

<span class="sd">        Inputs:</span>
<span class="sd">            se3pose (math_helpers.SE3Pose)</span>

<span class="sd">        Returns:</span>
<span class="sd">            math_helpers.SE2Pose representing the flattened se3pose.</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="n">x</span> <span class="o">=</span> <span class="n">se3pose</span><span class="o">.</span><span class="n">x</span>
        <span class="n">y</span> <span class="o">=</span> <span class="n">se3pose</span><span class="o">.</span><span class="n">y</span>
        <span class="n">angle</span> <span class="o">=</span> <span class="n">se3pose</span><span class="o">.</span><span class="n">rot</span><span class="o">.</span><span class="n">to_yaw</span><span class="p">()</span>
        <span class="k">return</span> <span class="n">SE2Pose</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="n">angle</span><span class="p">)</span></div>

<div class="viewcode-block" id="SE2Pose.to_obj"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE2Pose.to_obj">[docs]</a>    <span class="k">def</span> <span class="nf">to_obj</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">proto</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Adds the math_helpers.SE2Pose properties into the geometry_pb2.SE2Pose &#39;proto&#39;.&quot;&quot;&quot;</span>
        <span class="n">proto</span><span class="o">.</span><span class="n">position</span><span class="o">.</span><span class="n">x</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">x</span>
        <span class="n">proto</span><span class="o">.</span><span class="n">position</span><span class="o">.</span><span class="n">y</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span>
        <span class="n">proto</span><span class="o">.</span><span class="n">angle</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">angle</span></div>

<div class="viewcode-block" id="SE2Pose.to_proto"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE2Pose.to_proto">[docs]</a>    <span class="k">def</span> <span class="nf">to_proto</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Converts the math_helpers.SE2Pose into an output of the protobuf geometry_pb2.SE2Pose.&quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="n">geometry_pb2</span><span class="o">.</span><span class="n">SE2Pose</span><span class="p">(</span><span class="n">position</span><span class="o">=</span><span class="n">geometry_pb2</span><span class="o">.</span><span class="n">Vec2</span><span class="p">(</span><span class="n">x</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">y</span><span class="p">),</span>
                                    <span class="n">angle</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">angle</span><span class="p">)</span></div>

<div class="viewcode-block" id="SE2Pose.inverse"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE2Pose.inverse">[docs]</a>    <span class="k">def</span> <span class="nf">inverse</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Compute the inverse of the math_helpers.SE2Pose.</span>

<span class="sd">        For example, if the SE(2) pose represented a_tform_b, then the inverse pose is b_tform_a.</span>

<span class="sd">        Returns:</span>
<span class="sd">            math_helpers.SE2Pose representing the inverse of the current SE(2) Pose.</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="n">c</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">angle</span><span class="p">)</span>
        <span class="n">s</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">angle</span><span class="p">)</span>
        <span class="k">return</span> <span class="n">SE2Pose</span><span class="p">(</span><span class="o">-</span><span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">*</span> <span class="n">c</span> <span class="o">-</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">*</span> <span class="n">s</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">*</span> <span class="n">s</span> <span class="o">-</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">*</span> <span class="n">c</span><span class="p">,</span> <span class="o">-</span><span class="bp">self</span><span class="o">.</span><span class="n">angle</span><span class="p">)</span></div>

<div class="viewcode-block" id="SE2Pose.mult"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE2Pose.mult">[docs]</a>    <span class="k">def</span> <span class="nf">mult</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">se2pose</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Computes the multiplication between the current math_helpers.SE2Pose and the input se2pose.</span>

<span class="sd">        For example, if the &#39;self&#39; SE2Pose represents a_tform_b and the input se2pose represents b_tform_c,</span>
<span class="sd">        then the output will represent the transform a_tform_c.</span>

<span class="sd">        Inputs:</span>
<span class="sd">            se2pose (math_helpers.se2pose)</span>

<span class="sd">        Returns:</span>
<span class="sd">            math_helpers.se2pose representing the multiplication of two SE(2) poses.</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="n">rotation_matrix</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">to_rot_matrix</span><span class="p">()</span>
        <span class="n">rotated_pos</span> <span class="o">=</span> <span class="n">rotation_matrix</span><span class="o">.</span><span class="n">dot</span><span class="p">((</span><span class="n">se2pose</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="n">se2pose</span><span class="o">.</span><span class="n">y</span><span class="p">))</span>
        <span class="k">return</span> <span class="n">SE2Pose</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">+</span> <span class="n">rotated_pos</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">+</span> <span class="n">rotated_pos</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span>
                       <span class="n">recenter_angle_mod</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">angle</span> <span class="o">+</span> <span class="n">se2pose</span><span class="o">.</span><span class="n">angle</span><span class="p">,</span> <span class="mf">0.0</span><span class="p">))</span></div>

    <span class="k">def</span> <span class="fm">__mul__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">other</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Overrides the &#39;*&#39; symbol to compute the multiplication between two SE(2) poses,</span>
<span class="sd">        or between an SE(2) pose and a Vec2&quot;&quot;&quot;</span>
        <span class="k">if</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">other</span><span class="p">,</span> <span class="n">Vec2</span><span class="p">):</span>
            <span class="n">rotation_matrix</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">to_rot_matrix</span><span class="p">()</span>
            <span class="n">rotated_pos</span> <span class="o">=</span> <span class="n">rotation_matrix</span><span class="o">.</span><span class="n">dot</span><span class="p">((</span><span class="n">other</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="n">other</span><span class="o">.</span><span class="n">y</span><span class="p">))</span>
            <span class="k">return</span> <span class="n">Vec2</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">+</span> <span class="n">rotated_pos</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">+</span> <span class="n">rotated_pos</span><span class="p">[</span><span class="mi">1</span><span class="p">])</span>
        <span class="k">if</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">other</span><span class="p">,</span> <span class="n">SE2Pose</span><span class="p">):</span>
            <span class="n">rotation_matrix</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">to_rot_matrix</span><span class="p">()</span>
            <span class="n">rotated_pos</span> <span class="o">=</span> <span class="n">rotation_matrix</span><span class="o">.</span><span class="n">dot</span><span class="p">((</span><span class="n">other</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="n">other</span><span class="o">.</span><span class="n">y</span><span class="p">))</span>
            <span class="k">return</span> <span class="n">SE2Pose</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">+</span> <span class="n">rotated_pos</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">+</span> <span class="n">rotated_pos</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span>
                           <span class="n">recenter_angle_mod</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">angle</span> <span class="o">+</span> <span class="n">other</span><span class="o">.</span><span class="n">angle</span><span class="p">,</span> <span class="mf">0.0</span><span class="p">))</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="k">raise</span> <span class="ne">TypeError</span><span class="p">(</span><span class="s2">&quot;Can&#39;t multiply types </span><span class="si">%s</span><span class="s2"> and </span><span class="si">%s</span><span class="s2">.&quot;</span> <span class="o">%</span> <span class="p">(</span><span class="nb">type</span><span class="p">(</span><span class="bp">self</span><span class="p">),</span> <span class="nb">type</span><span class="p">(</span><span class="n">other</span><span class="p">)))</span>

<div class="viewcode-block" id="SE2Pose.to_rot_matrix"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE2Pose.to_rot_matrix">[docs]</a>    <span class="k">def</span> <span class="nf">to_rot_matrix</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Returns the rotation matrix generate from the angle of the current SE(2) Pose.&quot;&quot;&quot;</span>
        <span class="n">c</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">angle</span><span class="p">)</span>
        <span class="n">s</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">angle</span><span class="p">)</span>
        <span class="k">return</span> <span class="n">numpy</span><span class="o">.</span><span class="n">array</span><span class="p">([[</span><span class="n">c</span><span class="p">,</span> <span class="o">-</span><span class="n">s</span><span class="p">],</span> <span class="p">[</span><span class="n">s</span><span class="p">,</span> <span class="n">c</span><span class="p">]])</span></div>

<div class="viewcode-block" id="SE2Pose.to_matrix"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE2Pose.to_matrix">[docs]</a>    <span class="k">def</span> <span class="nf">to_matrix</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Returns the 3x3 matrix to transform a 2D point (in generalized coordinates).&quot;&quot;&quot;</span>
        <span class="n">c</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">angle</span><span class="p">)</span>
        <span class="n">s</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">angle</span><span class="p">)</span>
        <span class="k">return</span> <span class="n">numpy</span><span class="o">.</span><span class="n">array</span><span class="p">([[</span><span class="n">c</span><span class="p">,</span> <span class="o">-</span><span class="n">s</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">x</span><span class="p">],</span> <span class="p">[</span><span class="n">s</span><span class="p">,</span> <span class="n">c</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span><span class="p">],</span> <span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">]])</span></div>

<div class="viewcode-block" id="SE2Pose.to_adjoint_matrix"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE2Pose.to_adjoint_matrix">[docs]</a>    <span class="k">def</span> <span class="nf">to_adjoint_matrix</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;This creates the adjoint matrix for the current SE2Pose.</span>

<span class="sd">        The adjoint matrix can be used to change reference frames for a SE(2) velocity vector. For</span>
<span class="sd">        example, if you have math_helpers.SE2Velocity velocity_in_frame_b, then the adjoint matrix</span>
<span class="sd">        for the math_helpers.SE2Pose (representing a_tform_b) can be used as follows to transform</span>
<span class="sd">        the velocity: velocity_in_frame_a = a_tform_b.to_adjoint_matrix() * velocity_in_frame_b</span>

<span class="sd">        Returns:</span>
<span class="sd">            a_adjoint_b (Numpy 3x3 matrix) representing the adjoint matrix for the SE2Pose.</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="n">a_R_b</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">to_rot_matrix</span><span class="p">()</span>
        <span class="n">position_skew_mat</span> <span class="o">=</span> <span class="n">skew_matrix_2d</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">position</span><span class="p">)</span>
        <span class="n">a_adjoint_b</span> <span class="o">=</span> <span class="n">numpy</span><span class="o">.</span><span class="n">block</span><span class="p">([[</span><span class="n">a_R_b</span><span class="p">,</span> <span class="n">position_skew_mat</span><span class="o">.</span><span class="n">T</span><span class="p">],</span> <span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">]])</span>
        <span class="k">return</span> <span class="n">a_adjoint_b</span></div>

    <span class="nd">@property</span>
    <span class="k">def</span> <span class="nf">position</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Property to allow attribute access of the protobuf message field &#39;position&#39; similar to</span>
<span class="sd">        the geometry_pb2.SE2Pose for the math_helper.SE2Pose.&quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="n">geometry_pb2</span><span class="o">.</span><span class="n">Vec2</span><span class="p">(</span><span class="n">x</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">y</span><span class="p">)</span>

<div class="viewcode-block" id="SE2Pose.from_matrix"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE2Pose.from_matrix">[docs]</a>    <span class="nd">@staticmethod</span>
    <span class="k">def</span> <span class="nf">from_matrix</span><span class="p">(</span><span class="n">mat</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Extract SE2Pose from a 3x3 matrix&quot;&quot;&quot;</span>
        <span class="c1"># Extract separately to get float type, instead of single-element matrix type.</span>
        <span class="n">x</span> <span class="o">=</span> <span class="n">mat</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">2</span><span class="p">]</span>
        <span class="n">y</span> <span class="o">=</span> <span class="n">mat</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">2</span><span class="p">]</span>
        <span class="n">angle</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="n">mat</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">mat</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">])</span>
        <span class="nb">print</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="n">angle</span><span class="p">)</span>
        <span class="k">return</span> <span class="n">SE2Pose</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="n">angle</span><span class="p">)</span></div>

<div class="viewcode-block" id="SE2Pose.from_obj"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE2Pose.from_obj">[docs]</a>    <span class="nd">@staticmethod</span>
    <span class="nd">@deprecated</span><span class="p">(</span><span class="n">reason</span><span class="o">=</span><span class="s1">&#39;Use from_proto instead.&#39;</span><span class="p">,</span> <span class="n">version</span><span class="o">=</span><span class="s1">&#39;3.1.0&#39;</span><span class="p">)</span>
    <span class="k">def</span> <span class="nf">from_obj</span><span class="p">(</span><span class="n">tform</span><span class="p">):</span>
        <span class="k">return</span> <span class="n">SE2Pose</span><span class="o">.</span><span class="n">from_proto</span><span class="p">(</span><span class="n">tform</span><span class="p">)</span></div>

<div class="viewcode-block" id="SE2Pose.from_proto"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE2Pose.from_proto">[docs]</a>    <span class="nd">@staticmethod</span>
    <span class="k">def</span> <span class="nf">from_proto</span><span class="p">(</span><span class="n">tform</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Create a math_helpers.SE2Pose from a geometry_pb2.SE2Pose proto.&quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="n">SE2Pose</span><span class="p">(</span><span class="n">tform</span><span class="o">.</span><span class="n">position</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="n">tform</span><span class="o">.</span><span class="n">position</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="n">tform</span><span class="o">.</span><span class="n">angle</span><span class="p">)</span></div>

<div class="viewcode-block" id="SE2Pose.get_closest_se3_transform"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE2Pose.get_closest_se3_transform">[docs]</a>    <span class="k">def</span> <span class="nf">get_closest_se3_transform</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">height_z</span><span class="o">=</span><span class="mf">0.0</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Compute the closest math_helpers.SE3Pose from the current math_helpers.SE2Pose.&quot;&quot;&quot;</span>
        <span class="c1"># Note that if the SE(2) pose is a transform from a non-gravity-aligned frame,</span>
        <span class="c1"># then height information will be incorrect in the inflated SE(3) pose unless</span>
        <span class="c1"># the correct height is passed into the function as height_z.</span>
        <span class="k">return</span> <span class="n">SE3Pose</span><span class="p">(</span><span class="n">x</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="n">z</span><span class="o">=</span><span class="n">height_z</span><span class="p">,</span> <span class="n">rot</span><span class="o">=</span><span class="n">Quat</span><span class="o">.</span><span class="n">from_yaw</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">angle</span><span class="p">))</span></div></div>


<div class="viewcode-block" id="SE2Velocity"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE2Velocity">[docs]</a><span class="k">class</span> <span class="nc">SE2Velocity</span><span class="p">(</span><span class="nb">object</span><span class="p">):</span>
<span class="w">    </span><span class="sd">&quot;&quot;&quot;Class representing an SE2Velocity with linear velocity and angular velocity.&quot;&quot;&quot;</span>

    <span class="k">def</span> <span class="fm">__init__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="n">angular</span><span class="p">):</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">linear_velocity_x</span> <span class="o">=</span> <span class="nb">float</span><span class="p">(</span><span class="n">x</span><span class="p">)</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">linear_velocity_y</span> <span class="o">=</span> <span class="nb">float</span><span class="p">(</span><span class="n">y</span><span class="p">)</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">angular_velocity</span> <span class="o">=</span> <span class="nb">float</span><span class="p">(</span><span class="n">angular</span><span class="p">)</span>

    <span class="k">def</span> <span class="fm">__str__</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
        <span class="k">return</span> <span class="s1">&#39;Linear velocity -- X: </span><span class="si">%0.3f</span><span class="s1"> Y: </span><span class="si">%0.3f</span><span class="s1"> Angular velocity -- </span><span class="si">%0.3f</span><span class="s1"> &#39;</span> <span class="o">%</span> <span class="p">(</span>
            <span class="bp">self</span><span class="o">.</span><span class="n">linear_velocity_x</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">linear_velocity_y</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">angular_velocity</span><span class="p">)</span>

<div class="viewcode-block" id="SE2Velocity.to_obj"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE2Velocity.to_obj">[docs]</a>    <span class="k">def</span> <span class="nf">to_obj</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">proto</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Adds the math_helpers.SE2Velocity properties into the geometry_pb2.SE2Velocity &#39;proto&#39;.&quot;&quot;&quot;</span>
        <span class="n">proto</span><span class="o">.</span><span class="n">linear</span><span class="o">.</span><span class="n">x</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">linear_velocity_x</span>
        <span class="n">proto</span><span class="o">.</span><span class="n">linear</span><span class="o">.</span><span class="n">y</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">linear_velocity_y</span>
        <span class="n">proto</span><span class="o">.</span><span class="n">angular</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">angular_velocity</span></div>

<div class="viewcode-block" id="SE2Velocity.to_proto"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE2Velocity.to_proto">[docs]</a>    <span class="k">def</span> <span class="nf">to_proto</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Converts the math_helpers.SE2Velocity into an output of the protobuf geometry_pb2.SE2Velocity.&quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="n">geometry_pb2</span><span class="o">.</span><span class="n">SE2Velocity</span><span class="p">(</span>
            <span class="n">linear</span><span class="o">=</span><span class="n">geometry_pb2</span><span class="o">.</span><span class="n">Vec2</span><span class="p">(</span><span class="n">x</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">linear_velocity_x</span><span class="p">,</span> <span class="n">y</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">linear_velocity_y</span><span class="p">),</span>
            <span class="n">angular</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">angular_velocity</span><span class="p">)</span></div>

<div class="viewcode-block" id="SE2Velocity.to_vector"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE2Velocity.to_vector">[docs]</a>    <span class="k">def</span> <span class="nf">to_vector</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Creates a 3x1 velocity vector as a numpy array.&quot;&quot;&quot;</span>
        <span class="c1"># Create a 3x1 vector of [x_d, y_d, r_d]</span>
        <span class="k">return</span> <span class="n">numpy</span><span class="o">.</span><span class="n">array</span><span class="p">([</span><span class="bp">self</span><span class="o">.</span><span class="n">linear_velocity_x</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">linear_velocity_y</span><span class="p">,</span>
                            <span class="bp">self</span><span class="o">.</span><span class="n">angular_velocity</span><span class="p">])</span><span class="o">.</span><span class="n">reshape</span><span class="p">((</span><span class="mi">3</span><span class="p">,</span> <span class="mi">1</span><span class="p">))</span></div>

<div class="viewcode-block" id="SE2Velocity.from_vector"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE2Velocity.from_vector">[docs]</a>    <span class="nd">@staticmethod</span>
    <span class="k">def</span> <span class="nf">from_vector</span><span class="p">(</span><span class="n">se2_vel_vector</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Converts a 3x1 velocity vector (of either a numpy array or a list) into a math_helpers.SE2Velocity object.&quot;&quot;&quot;</span>
        <span class="k">if</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">se2_vel_vector</span><span class="p">,</span> <span class="nb">list</span><span class="p">):</span>
            <span class="k">if</span> <span class="nb">len</span><span class="p">(</span><span class="n">se2_vel_vector</span><span class="p">)</span> <span class="o">!=</span> <span class="mi">3</span><span class="p">:</span>
                <span class="c1"># Must have 3 elements to be a complete SE2Velocity</span>
                <span class="nb">print</span><span class="p">(</span><span class="s2">&quot;Velocity list must have 3 elements. The input has the wrong dimension of: &quot;</span> <span class="o">+</span>
                      <span class="nb">str</span><span class="p">(</span><span class="nb">len</span><span class="p">(</span><span class="n">se2_vel_vector</span><span class="p">)))</span>
                <span class="k">return</span> <span class="kc">None</span>
            <span class="k">else</span><span class="p">:</span>
                <span class="k">return</span> <span class="n">SE2Velocity</span><span class="p">(</span><span class="n">x</span><span class="o">=</span><span class="n">se2_vel_vector</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">y</span><span class="o">=</span><span class="n">se2_vel_vector</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span>
                                   <span class="n">angular</span><span class="o">=</span><span class="n">se2_vel_vector</span><span class="p">[</span><span class="mi">2</span><span class="p">])</span>
        <span class="k">if</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">se2_vel_vector</span><span class="p">,</span> <span class="n">numpy</span><span class="o">.</span><span class="n">ndarray</span><span class="p">):</span>
            <span class="k">if</span> <span class="n">se2_vel_vector</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">!=</span> <span class="mi">3</span><span class="p">:</span>
                <span class="c1"># Must have 3 elements to be a complete SE2Velocity</span>
                <span class="nb">print</span><span class="p">(</span>
                    <span class="s2">&quot;Velocity numpy array must have 3 elements. The input has the wrong dimension of: &quot;</span>
                    <span class="o">+</span> <span class="nb">str</span><span class="p">(</span><span class="n">se2_vel_vector</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">]))</span>
                <span class="k">return</span> <span class="kc">None</span>
            <span class="k">else</span><span class="p">:</span>
                <span class="k">if</span> <span class="p">(</span><span class="nb">isinstance</span><span class="p">(</span><span class="n">se2_vel_vector</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">numpy</span><span class="o">.</span><span class="n">ndarray</span><span class="p">)):</span>
                    <span class="k">return</span> <span class="n">SE2Velocity</span><span class="p">(</span><span class="n">x</span><span class="o">=</span><span class="n">se2_vel_vector</span><span class="p">[</span><span class="mi">0</span><span class="p">][</span><span class="mi">0</span><span class="p">],</span> <span class="n">y</span><span class="o">=</span><span class="n">se2_vel_vector</span><span class="p">[</span><span class="mi">1</span><span class="p">][</span><span class="mi">0</span><span class="p">],</span>
                                       <span class="n">angular</span><span class="o">=</span><span class="n">se2_vel_vector</span><span class="p">[</span><span class="mi">2</span><span class="p">][</span><span class="mi">0</span><span class="p">])</span>

                <span class="k">return</span> <span class="n">SE2Velocity</span><span class="p">(</span><span class="n">x</span><span class="o">=</span><span class="n">se2_vel_vector</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">y</span><span class="o">=</span><span class="n">se2_vel_vector</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span>
                                   <span class="n">angular</span><span class="o">=</span><span class="n">se2_vel_vector</span><span class="p">[</span><span class="mi">2</span><span class="p">])</span></div>

    <span class="nd">@property</span>
    <span class="k">def</span> <span class="nf">linear</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Property to allow attribute access of the protobuf message field &#39;linear&#39; similar to the geometry_pb2.SE2Velocity</span>
<span class="sd">           for the math_helper.SE2Velocity.&quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="n">geometry_pb2</span><span class="o">.</span><span class="n">Vec2</span><span class="p">(</span><span class="n">x</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">linear_velocity_x</span><span class="p">,</span> <span class="n">y</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">linear_velocity_y</span><span class="p">)</span>

    <span class="nd">@property</span>
    <span class="k">def</span> <span class="nf">angular</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Property to allow attribute access of the protobuf message field &#39;angular&#39; similar to the geometry_pb2.SE2Velocity</span>
<span class="sd">           for the math_helper.SE2Velocity.&quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="n">angular_velocity</span>

<div class="viewcode-block" id="SE2Velocity.from_obj"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE2Velocity.from_obj">[docs]</a>    <span class="nd">@staticmethod</span>
    <span class="nd">@deprecated</span><span class="p">(</span><span class="n">reason</span><span class="o">=</span><span class="s1">&#39;Use from_proto instead.&#39;</span><span class="p">,</span> <span class="n">version</span><span class="o">=</span><span class="s1">&#39;3.1.0&#39;</span><span class="p">)</span>
    <span class="k">def</span> <span class="nf">from_obj</span><span class="p">(</span><span class="n">vel</span><span class="p">):</span>
        <span class="k">return</span> <span class="n">SE2Velocity</span><span class="o">.</span><span class="n">from_proto</span><span class="p">(</span><span class="n">vel</span><span class="p">)</span></div>

<div class="viewcode-block" id="SE2Velocity.from_proto"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE2Velocity.from_proto">[docs]</a>    <span class="nd">@staticmethod</span>
    <span class="k">def</span> <span class="nf">from_proto</span><span class="p">(</span><span class="n">vel</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Create a math_helpers.SE2Velocity from a geometry_pb2.SE2Velocity proto.&quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="n">SE2Velocity</span><span class="p">(</span><span class="n">x</span><span class="o">=</span><span class="n">vel</span><span class="o">.</span><span class="n">linear</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="o">=</span><span class="n">vel</span><span class="o">.</span><span class="n">linear</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="n">angular</span><span class="o">=</span><span class="n">vel</span><span class="o">.</span><span class="n">angular</span><span class="p">)</span></div></div>


<div class="viewcode-block" id="SE3Velocity"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE3Velocity">[docs]</a><span class="k">class</span> <span class="nc">SE3Velocity</span><span class="p">(</span><span class="nb">object</span><span class="p">):</span>
<span class="w">    </span><span class="sd">&quot;&quot;&quot;Class representing an SE3Velocity with linear velocity and angular velocity.&quot;&quot;&quot;</span>

    <span class="k">def</span> <span class="fm">__init__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">lin_x</span><span class="p">,</span> <span class="n">lin_y</span><span class="p">,</span> <span class="n">lin_z</span><span class="p">,</span> <span class="n">ang_x</span><span class="p">,</span> <span class="n">ang_y</span><span class="p">,</span> <span class="n">ang_z</span><span class="p">):</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">linear_velocity_x</span> <span class="o">=</span> <span class="nb">float</span><span class="p">(</span><span class="n">lin_x</span><span class="p">)</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">linear_velocity_y</span> <span class="o">=</span> <span class="nb">float</span><span class="p">(</span><span class="n">lin_y</span><span class="p">)</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">linear_velocity_z</span> <span class="o">=</span> <span class="nb">float</span><span class="p">(</span><span class="n">lin_z</span><span class="p">)</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">angular_velocity_x</span> <span class="o">=</span> <span class="nb">float</span><span class="p">(</span><span class="n">ang_x</span><span class="p">)</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">angular_velocity_y</span> <span class="o">=</span> <span class="nb">float</span><span class="p">(</span><span class="n">ang_y</span><span class="p">)</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">angular_velocity_z</span> <span class="o">=</span> <span class="nb">float</span><span class="p">(</span><span class="n">ang_z</span><span class="p">)</span>

    <span class="k">def</span> <span class="fm">__str__</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
        <span class="k">return</span> <span class="s1">&#39;Linear velocity -- X: </span><span class="si">%0.3f</span><span class="s1"> Y: </span><span class="si">%0.3f</span><span class="s1"> Z: </span><span class="si">%0.3f</span><span class="s1"> Angular velocity -- X: </span><span class="si">%0.3f</span><span class="s1"> Y: </span><span class="si">%0.3f</span><span class="s1"> Z: </span><span class="si">%0.3f</span><span class="s1">&#39;</span> <span class="o">%</span> <span class="p">(</span>
            <span class="bp">self</span><span class="o">.</span><span class="n">linear_velocity_x</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">linear_velocity_y</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">linear_velocity_z</span><span class="p">,</span>
            <span class="bp">self</span><span class="o">.</span><span class="n">angular_velocity_x</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">angular_velocity_y</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">angular_velocity_z</span><span class="p">)</span>

<div class="viewcode-block" id="SE3Velocity.to_obj"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE3Velocity.to_obj">[docs]</a>    <span class="k">def</span> <span class="nf">to_obj</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">proto</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Adds the math_helpers.SE3Velocity properties into the geometry_pb2.SE3Velocity &#39;proto&#39;.&quot;&quot;&quot;</span>
        <span class="n">proto</span><span class="o">.</span><span class="n">linear</span><span class="o">.</span><span class="n">x</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">linear_velocity_x</span>
        <span class="n">proto</span><span class="o">.</span><span class="n">linear</span><span class="o">.</span><span class="n">y</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">linear_velocity_y</span>
        <span class="n">proto</span><span class="o">.</span><span class="n">linear</span><span class="o">.</span><span class="n">z</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">linear_velocity_z</span>
        <span class="n">proto</span><span class="o">.</span><span class="n">angular</span><span class="o">.</span><span class="n">x</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">angular_velocity_x</span>
        <span class="n">proto</span><span class="o">.</span><span class="n">angular</span><span class="o">.</span><span class="n">y</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">angular_velocity_y</span>
        <span class="n">proto</span><span class="o">.</span><span class="n">angular</span><span class="o">.</span><span class="n">z</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">angular_velocity_z</span></div>

<div class="viewcode-block" id="SE3Velocity.to_proto"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE3Velocity.to_proto">[docs]</a>    <span class="k">def</span> <span class="nf">to_proto</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Converts the math_helpers.SE3Velocity into an output of the protobuf geometry_pb2.SE3Velocity.&quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="n">geometry_pb2</span><span class="o">.</span><span class="n">SE3Velocity</span><span class="p">(</span>
            <span class="n">linear</span><span class="o">=</span><span class="n">geometry_pb2</span><span class="o">.</span><span class="n">Vec3</span><span class="p">(</span><span class="n">x</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">linear_velocity_x</span><span class="p">,</span> <span class="n">y</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">linear_velocity_y</span><span class="p">,</span>
                                     <span class="n">z</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">linear_velocity_z</span><span class="p">),</span>
            <span class="n">angular</span><span class="o">=</span><span class="n">geometry_pb2</span><span class="o">.</span><span class="n">Vec3</span><span class="p">(</span><span class="n">x</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">angular_velocity_x</span><span class="p">,</span> <span class="n">y</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">angular_velocity_y</span><span class="p">,</span>
                                      <span class="n">z</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">angular_velocity_z</span><span class="p">))</span></div>

<div class="viewcode-block" id="SE3Velocity.to_vector"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE3Velocity.to_vector">[docs]</a>    <span class="k">def</span> <span class="nf">to_vector</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Creates a 6x1 velocity vector as a numpy array.&quot;&quot;&quot;</span>
        <span class="c1"># Create a vector [x_d, y_d, z_d, r_x_d, r_y_d, r_z_d] of dimensions 6x1</span>
        <span class="k">return</span> <span class="n">numpy</span><span class="o">.</span><span class="n">array</span><span class="p">([</span>
            <span class="bp">self</span><span class="o">.</span><span class="n">linear_velocity_x</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">linear_velocity_y</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">linear_velocity_z</span><span class="p">,</span>
            <span class="bp">self</span><span class="o">.</span><span class="n">angular_velocity_x</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">angular_velocity_y</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">angular_velocity_z</span>
        <span class="p">])</span><span class="o">.</span><span class="n">reshape</span><span class="p">((</span><span class="mi">6</span><span class="p">,</span> <span class="mi">1</span><span class="p">))</span></div>

    <span class="nd">@property</span>
    <span class="k">def</span> <span class="nf">linear</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Property to allow attribute access of the protobuf message field &#39;linear&#39; similar to the geometry_pb2.SE3Velocity</span>
<span class="sd">           for the math_helper.SE3Velocity.&quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="n">geometry_pb2</span><span class="o">.</span><span class="n">Vec3</span><span class="p">(</span><span class="n">x</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">linear_velocity_x</span><span class="p">,</span> <span class="n">y</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">linear_velocity_y</span><span class="p">,</span>
                                 <span class="n">z</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">linear_velocity_z</span><span class="p">)</span>

    <span class="nd">@property</span>
    <span class="k">def</span> <span class="nf">angular</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Property to allow attribute access of the protobuf message field &#39;angular&#39; similar to the geometry_pb2.SE3Velocity</span>
<span class="sd">           for the math_helper.SE3Velocity.&quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="n">geometry_pb2</span><span class="o">.</span><span class="n">Vec3</span><span class="p">(</span><span class="n">x</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">angular_velocity_x</span><span class="p">,</span> <span class="n">y</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">angular_velocity_y</span><span class="p">,</span>
                                 <span class="n">z</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">angular_velocity_z</span><span class="p">)</span>

<div class="viewcode-block" id="SE3Velocity.from_obj"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE3Velocity.from_obj">[docs]</a>    <span class="nd">@staticmethod</span>
    <span class="nd">@deprecated</span><span class="p">(</span><span class="n">reason</span><span class="o">=</span><span class="s1">&#39;Use from_proto instead.&#39;</span><span class="p">,</span> <span class="n">version</span><span class="o">=</span><span class="s1">&#39;3.1.0&#39;</span><span class="p">)</span>
    <span class="k">def</span> <span class="nf">from_obj</span><span class="p">(</span><span class="n">vel</span><span class="p">):</span>
        <span class="k">return</span> <span class="n">SE3Velocity</span><span class="o">.</span><span class="n">from_proto</span><span class="p">(</span><span class="n">vel</span><span class="p">)</span></div>

<div class="viewcode-block" id="SE3Velocity.from_proto"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE3Velocity.from_proto">[docs]</a>    <span class="nd">@staticmethod</span>
    <span class="k">def</span> <span class="nf">from_proto</span><span class="p">(</span><span class="n">vel</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Create a math_helpers.SE3Velocity from a geometry_pb2.SE3Velocity proto.&quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="n">SE3Velocity</span><span class="p">(</span><span class="n">lin_x</span><span class="o">=</span><span class="n">vel</span><span class="o">.</span><span class="n">linear</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="n">lin_y</span><span class="o">=</span><span class="n">vel</span><span class="o">.</span><span class="n">linear</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="n">lin_z</span><span class="o">=</span><span class="n">vel</span><span class="o">.</span><span class="n">linear</span><span class="o">.</span><span class="n">z</span><span class="p">,</span>
                           <span class="n">ang_x</span><span class="o">=</span><span class="n">vel</span><span class="o">.</span><span class="n">angular</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="n">ang_y</span><span class="o">=</span><span class="n">vel</span><span class="o">.</span><span class="n">angular</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="n">ang_z</span><span class="o">=</span><span class="n">vel</span><span class="o">.</span><span class="n">angular</span><span class="o">.</span><span class="n">z</span><span class="p">)</span></div>

<div class="viewcode-block" id="SE3Velocity.from_vector"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE3Velocity.from_vector">[docs]</a>    <span class="nd">@staticmethod</span>
    <span class="k">def</span> <span class="nf">from_vector</span><span class="p">(</span><span class="n">se3_vel_vector</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Converts a 6x1 velocity vector (of either a numpy array or a list) into a math_helpers.SE3Velocity object.&quot;&quot;&quot;</span>
        <span class="k">if</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">se3_vel_vector</span><span class="p">,</span> <span class="nb">list</span><span class="p">):</span>
            <span class="k">if</span> <span class="nb">len</span><span class="p">(</span><span class="n">se3_vel_vector</span><span class="p">)</span> <span class="o">!=</span> <span class="mi">6</span><span class="p">:</span>
                <span class="c1"># Must have 6 elements to be a complete SE3Velocity</span>
                <span class="nb">print</span><span class="p">(</span><span class="s2">&quot;Velocity list must have 6 elements. The input has the wrong dimension of: &quot;</span> <span class="o">+</span>
                      <span class="nb">str</span><span class="p">(</span><span class="nb">len</span><span class="p">(</span><span class="n">se3_vel_vector</span><span class="p">)))</span>
                <span class="k">return</span> <span class="kc">None</span>
            <span class="k">else</span><span class="p">:</span>
                <span class="k">return</span> <span class="n">SE3Velocity</span><span class="p">(</span><span class="n">lin_x</span><span class="o">=</span><span class="n">se3_vel_vector</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">lin_y</span><span class="o">=</span><span class="n">se3_vel_vector</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span>
                                   <span class="n">lin_z</span><span class="o">=</span><span class="n">se3_vel_vector</span><span class="p">[</span><span class="mi">2</span><span class="p">],</span> <span class="n">ang_x</span><span class="o">=</span><span class="n">se3_vel_vector</span><span class="p">[</span><span class="mi">3</span><span class="p">],</span>
                                   <span class="n">ang_y</span><span class="o">=</span><span class="n">se3_vel_vector</span><span class="p">[</span><span class="mi">4</span><span class="p">],</span> <span class="n">ang_z</span><span class="o">=</span><span class="n">se3_vel_vector</span><span class="p">[</span><span class="mi">5</span><span class="p">])</span>
        <span class="k">if</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">se3_vel_vector</span><span class="p">,</span> <span class="n">numpy</span><span class="o">.</span><span class="n">ndarray</span><span class="p">):</span>
            <span class="k">if</span> <span class="n">se3_vel_vector</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">!=</span> <span class="mi">6</span><span class="p">:</span>
                <span class="c1"># Must have 6 elements to be a complete SE3Velocity</span>
                <span class="nb">print</span><span class="p">(</span>
                    <span class="s2">&quot;Velocity numpy array must have 6 elements. The input has the wrong dimension of: &quot;</span>
                    <span class="o">+</span> <span class="nb">str</span><span class="p">(</span><span class="n">se3_vel_vector</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">]))</span>
                <span class="k">return</span> <span class="kc">None</span>
            <span class="k">else</span><span class="p">:</span>
                <span class="k">if</span> <span class="p">(</span><span class="nb">isinstance</span><span class="p">(</span><span class="n">se3_vel_vector</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">numpy</span><span class="o">.</span><span class="n">ndarray</span><span class="p">)):</span>
                    <span class="k">return</span> <span class="n">SE3Velocity</span><span class="p">(</span><span class="n">lin_x</span><span class="o">=</span><span class="n">se3_vel_vector</span><span class="p">[</span><span class="mi">0</span><span class="p">][</span><span class="mi">0</span><span class="p">],</span> <span class="n">lin_y</span><span class="o">=</span><span class="n">se3_vel_vector</span><span class="p">[</span><span class="mi">1</span><span class="p">][</span><span class="mi">0</span><span class="p">],</span>
                                       <span class="n">lin_z</span><span class="o">=</span><span class="n">se3_vel_vector</span><span class="p">[</span><span class="mi">2</span><span class="p">][</span><span class="mi">0</span><span class="p">],</span> <span class="n">ang_x</span><span class="o">=</span><span class="n">se3_vel_vector</span><span class="p">[</span><span class="mi">3</span><span class="p">][</span><span class="mi">0</span><span class="p">],</span>
                                       <span class="n">ang_y</span><span class="o">=</span><span class="n">se3_vel_vector</span><span class="p">[</span><span class="mi">4</span><span class="p">][</span><span class="mi">0</span><span class="p">],</span> <span class="n">ang_z</span><span class="o">=</span><span class="n">se3_vel_vector</span><span class="p">[</span><span class="mi">5</span><span class="p">][</span><span class="mi">0</span><span class="p">])</span>

                <span class="k">return</span> <span class="n">SE3Velocity</span><span class="p">(</span><span class="n">lin_x</span><span class="o">=</span><span class="n">se3_vel_vector</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">lin_y</span><span class="o">=</span><span class="n">se3_vel_vector</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span>
                                   <span class="n">lin_z</span><span class="o">=</span><span class="n">se3_vel_vector</span><span class="p">[</span><span class="mi">2</span><span class="p">],</span> <span class="n">ang_x</span><span class="o">=</span><span class="n">se3_vel_vector</span><span class="p">[</span><span class="mi">3</span><span class="p">],</span>
                                   <span class="n">ang_y</span><span class="o">=</span><span class="n">se3_vel_vector</span><span class="p">[</span><span class="mi">4</span><span class="p">],</span> <span class="n">ang_z</span><span class="o">=</span><span class="n">se3_vel_vector</span><span class="p">[</span><span class="mi">5</span><span class="p">])</span></div></div>


<div class="viewcode-block" id="SE3Pose"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE3Pose">[docs]</a><span class="k">class</span> <span class="nc">SE3Pose</span><span class="p">(</span><span class="nb">object</span><span class="p">):</span>
<span class="w">    </span><span class="sd">&quot;&quot;&quot;Class representing an SE3Pose with position and rotation.&quot;&quot;&quot;</span>

    <span class="k">def</span> <span class="fm">__init__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="n">z</span><span class="p">,</span> <span class="n">rot</span><span class="p">):</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">=</span> <span class="n">x</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">=</span> <span class="n">y</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">z</span> <span class="o">=</span> <span class="n">z</span>
        <span class="c1"># Expect the declaration of math_helpers.SE3Pose to pass a math_helpers.Quat, however we will convert</span>
        <span class="c1"># a protobuf Quaternion into the math_helpers object as well.</span>
        <span class="k">if</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">rot</span><span class="p">,</span> <span class="n">geometry_pb2</span><span class="o">.</span><span class="n">Quaternion</span><span class="p">):</span>
            <span class="n">rot</span> <span class="o">=</span> <span class="n">Quat</span><span class="o">.</span><span class="n">from_proto</span><span class="p">(</span><span class="n">rot</span><span class="p">)</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">rot</span> <span class="o">=</span> <span class="n">rot</span>

    <span class="k">def</span> <span class="fm">__str__</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
        <span class="k">return</span> <span class="s1">&#39;position -- X: </span><span class="si">%0.3f</span><span class="s1"> Y: </span><span class="si">%0.3f</span><span class="s1"> Z: </span><span class="si">%0.3f</span><span class="s1"> rotation -- </span><span class="si">%s</span><span class="s1">&#39;</span> <span class="o">%</span> <span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span><span class="p">,</span>
                                                                          <span class="nb">str</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">rot</span><span class="p">))</span>

    <span class="k">def</span> <span class="fm">__iter__</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
        <span class="k">return</span> <span class="nb">iter</span><span class="p">([</span><span class="bp">self</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">rot</span><span class="o">.</span><span class="n">w</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">rot</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">rot</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">rot</span><span class="o">.</span><span class="n">z</span><span class="p">])</span>

<div class="viewcode-block" id="SE3Pose.from_obj"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE3Pose.from_obj">[docs]</a>    <span class="nd">@staticmethod</span>
    <span class="nd">@deprecated</span><span class="p">(</span><span class="n">reason</span><span class="o">=</span><span class="s1">&#39;Use from_proto instead.&#39;</span><span class="p">,</span> <span class="n">version</span><span class="o">=</span><span class="s1">&#39;3.1.0&#39;</span><span class="p">)</span>
    <span class="k">def</span> <span class="nf">from_obj</span><span class="p">(</span><span class="n">tform</span><span class="p">):</span>
        <span class="k">return</span> <span class="n">SE3Pose</span><span class="o">.</span><span class="n">from_proto</span><span class="p">(</span><span class="n">tform</span><span class="p">)</span></div>

<div class="viewcode-block" id="SE3Pose.from_proto"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE3Pose.from_proto">[docs]</a>    <span class="nd">@staticmethod</span>
    <span class="k">def</span> <span class="nf">from_proto</span><span class="p">(</span><span class="n">tform</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Create a math_helpers.SE3Pose from a geometry_pb2.SE3Pose proto.&quot;&quot;&quot;</span>
        <span class="k">if</span> <span class="n">tform</span><span class="o">.</span><span class="n">HasField</span><span class="p">(</span><span class="s1">&#39;rotation&#39;</span><span class="p">):</span>
            <span class="n">quat</span> <span class="o">=</span> <span class="n">Quat</span><span class="o">.</span><span class="n">from_proto</span><span class="p">(</span><span class="n">tform</span><span class="o">.</span><span class="n">rotation</span><span class="p">)</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="c1"># Create the identity quaternion if no rotation is provided in the SE(3) pose.</span>
            <span class="n">quat</span> <span class="o">=</span> <span class="n">Quat</span><span class="p">()</span>
        <span class="k">return</span> <span class="n">SE3Pose</span><span class="p">(</span><span class="n">tform</span><span class="o">.</span><span class="n">position</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="n">tform</span><span class="o">.</span><span class="n">position</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="n">tform</span><span class="o">.</span><span class="n">position</span><span class="o">.</span><span class="n">z</span><span class="p">,</span> <span class="n">quat</span><span class="p">)</span></div>

<div class="viewcode-block" id="SE3Pose.from_se2"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE3Pose.from_se2">[docs]</a>    <span class="nd">@staticmethod</span>
    <span class="k">def</span> <span class="nf">from_se2</span><span class="p">(</span><span class="n">tform</span><span class="p">,</span> <span class="n">z</span><span class="o">=</span><span class="mi">0</span><span class="p">):</span>
        <span class="k">return</span> <span class="n">SE3Pose</span><span class="p">(</span><span class="n">tform</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="n">tform</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="n">z</span><span class="p">,</span> <span class="n">Quat</span><span class="o">.</span><span class="n">from_yaw</span><span class="p">(</span><span class="n">tform</span><span class="o">.</span><span class="n">angle</span><span class="p">))</span></div>

<div class="viewcode-block" id="SE3Pose.to_obj"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE3Pose.to_obj">[docs]</a>    <span class="k">def</span> <span class="nf">to_obj</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">proto</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Adds the math_helpers.SE3Pose properties into the geometry_pb2.SE3Pose &#39;proto&#39;.&quot;&quot;&quot;</span>
        <span class="n">proto</span><span class="o">.</span><span class="n">position</span><span class="o">.</span><span class="n">x</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">x</span>
        <span class="n">proto</span><span class="o">.</span><span class="n">position</span><span class="o">.</span><span class="n">y</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span>
        <span class="n">proto</span><span class="o">.</span><span class="n">position</span><span class="o">.</span><span class="n">z</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">rot</span><span class="o">.</span><span class="n">to_obj</span><span class="p">(</span><span class="n">proto</span><span class="o">.</span><span class="n">rotation</span><span class="p">)</span></div>

<div class="viewcode-block" id="SE3Pose.to_proto"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE3Pose.to_proto">[docs]</a>    <span class="k">def</span> <span class="nf">to_proto</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Converts the math_helpers.SE3Pose into an output of the protobuf geometry_pb2.SE3Pose.&quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="n">geometry_pb2</span><span class="o">.</span><span class="n">SE3Pose</span><span class="p">(</span>
            <span class="n">position</span><span class="o">=</span><span class="n">geometry_pb2</span><span class="o">.</span><span class="n">Vec3</span><span class="p">(</span><span class="n">x</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="n">z</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">z</span><span class="p">),</span>
            <span class="n">rotation</span><span class="o">=</span><span class="n">geometry_pb2</span><span class="o">.</span><span class="n">Quaternion</span><span class="p">(</span><span class="n">w</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">rot</span><span class="o">.</span><span class="n">w</span><span class="p">,</span> <span class="n">x</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">rot</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">rot</span><span class="o">.</span><span class="n">y</span><span class="p">,</span>
                                             <span class="n">z</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">rot</span><span class="o">.</span><span class="n">z</span><span class="p">))</span></div>

<div class="viewcode-block" id="SE3Pose.inverse"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE3Pose.inverse">[docs]</a>    <span class="k">def</span> <span class="nf">inverse</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Compute the inverse of the math_helpers.SE3Pose.</span>

<span class="sd">        For example, if the SE(3) pose represented a_tform_b, then the inverse pose is b_tform_a.</span>

<span class="sd">        Returns:</span>
<span class="sd">            math_helpers.SE3Pose representing the inverse of the current SE(3) Pose.</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="n">inv_rot</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">rot</span><span class="o">.</span><span class="n">inverse</span><span class="p">()</span>
        <span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="n">z</span><span class="p">)</span> <span class="o">=</span> <span class="n">inv_rot</span><span class="o">.</span><span class="n">transform_point</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span><span class="p">)</span>
        <span class="k">return</span> <span class="n">SE3Pose</span><span class="p">(</span><span class="o">-</span><span class="n">x</span><span class="p">,</span> <span class="o">-</span><span class="n">y</span><span class="p">,</span> <span class="o">-</span><span class="n">z</span><span class="p">,</span> <span class="n">inv_rot</span><span class="p">)</span></div>

<div class="viewcode-block" id="SE3Pose.transform_point"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE3Pose.transform_point">[docs]</a>    <span class="k">def</span> <span class="nf">transform_point</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="n">z</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Compute the transformation (translation and rotation) of a (x,y,z) vector using the</span>
<span class="sd">        current SE(3) pose.</span>

<span class="sd">        Returns:</span>
<span class="sd">            tuple (size 3) representing the transformed coordinate.</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="p">(</span><span class="n">out_x</span><span class="p">,</span> <span class="n">out_y</span><span class="p">,</span> <span class="n">out_z</span><span class="p">)</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">rot</span><span class="o">.</span><span class="n">transform_point</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="n">z</span><span class="p">)</span>
        <span class="k">return</span> <span class="p">(</span><span class="n">out_x</span> <span class="o">+</span> <span class="bp">self</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="n">out_y</span> <span class="o">+</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="n">out_z</span> <span class="o">+</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span><span class="p">)</span></div>

<div class="viewcode-block" id="SE3Pose.transform_vec3"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE3Pose.transform_vec3">[docs]</a>    <span class="k">def</span> <span class="nf">transform_vec3</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">vec3</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Compute the transformation (translation and rotation) of a (x,y,z) vector using the</span>
<span class="sd">        current SE(3) pose.</span>

<span class="sd">        Returns:</span>
<span class="sd">            Vec3 representing the transformed coordinate.</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="p">(</span><span class="n">out_x</span><span class="p">,</span> <span class="n">out_y</span><span class="p">,</span> <span class="n">out_z</span><span class="p">)</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">rot</span><span class="o">.</span><span class="n">transform_point</span><span class="p">(</span><span class="n">vec3</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="n">vec3</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="n">vec3</span><span class="o">.</span><span class="n">z</span><span class="p">)</span>
        <span class="k">return</span> <span class="n">geometry_pb2</span><span class="o">.</span><span class="n">Vec3</span><span class="p">(</span><span class="n">x</span><span class="o">=</span><span class="n">out_x</span> <span class="o">+</span> <span class="bp">self</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="o">=</span><span class="n">out_y</span> <span class="o">+</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="n">z</span><span class="o">=</span><span class="n">out_z</span> <span class="o">+</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span><span class="p">)</span></div>

<div class="viewcode-block" id="SE3Pose.transform_cloud"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE3Pose.transform_cloud">[docs]</a>    <span class="k">def</span> <span class="nf">transform_cloud</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">points</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Compute the transformation (translation and rotation) of multiple vector/points using the</span>
<span class="sd">        current math_helpers.SE3Pose.</span>

<span class="sd">        Inputs:</span>
<span class="sd">            points (Nx3 numpy matrix) representing a set of (x,y,z) points to be transformed</span>

<span class="sd">        Returns:</span>
<span class="sd">            Nx3 numpy matrix of the points after they are transformed with the current math_helpers.SE3Pose.</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="n">SE3Pose</span><span class="o">.</span><span class="n">transform_cloud_from_matrix</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">to_matrix</span><span class="p">(),</span> <span class="n">points</span><span class="p">)</span></div>

<div class="viewcode-block" id="SE3Pose.transform_cloud_from_matrix"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE3Pose.transform_cloud_from_matrix">[docs]</a>    <span class="nd">@staticmethod</span>
    <span class="k">def</span> <span class="nf">transform_cloud_from_matrix</span><span class="p">(</span><span class="n">transform</span><span class="p">,</span> <span class="n">points</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Compute the transformation (translation and rotation) of multiple vector/points using the</span>
<span class="sd">        input SE(3) pose.</span>

<span class="sd">        Inputs:</span>
<span class="sd">            transform (4x4 numpy matrix) representing the SE3Pose to be used to transform.</span>
<span class="sd">            points (Nx3 numpy matrix) representing a set of (x,y,z) points to be transformed</span>

<span class="sd">        Returns:</span>
<span class="sd">            Nx3 numpy matrix of the points after they are transformed.</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="n">rot</span> <span class="o">=</span> <span class="n">transform</span><span class="p">[</span><span class="mi">0</span><span class="p">:</span><span class="mi">3</span><span class="p">,</span> <span class="mi">0</span><span class="p">:</span><span class="mi">3</span><span class="p">]</span>
        <span class="n">trans</span> <span class="o">=</span> <span class="n">transform</span><span class="p">[</span><span class="mi">0</span><span class="p">:</span><span class="mi">3</span><span class="p">,</span> <span class="mi">3</span><span class="p">]</span>
        <span class="k">return</span> <span class="p">(</span><span class="n">numpy</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">points</span><span class="p">,</span> <span class="n">rot</span><span class="o">.</span><span class="n">T</span><span class="p">)</span> <span class="o">+</span> <span class="n">trans</span><span class="p">)</span></div>

<div class="viewcode-block" id="SE3Pose.to_matrix"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE3Pose.to_matrix">[docs]</a>    <span class="k">def</span> <span class="nf">to_matrix</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Returns the 4x4 matrix to transform a 3D point (in generalized coordinates).&quot;&quot;&quot;</span>
        <span class="n">ret</span> <span class="o">=</span> <span class="n">numpy</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">4</span><span class="p">)</span>
        <span class="n">ret</span><span class="p">[</span><span class="mi">0</span><span class="p">:</span><span class="mi">3</span><span class="p">,</span> <span class="mi">0</span><span class="p">:</span><span class="mi">3</span><span class="p">]</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">rot</span><span class="o">.</span><span class="n">to_matrix</span><span class="p">()</span>
        <span class="n">ret</span><span class="p">[</span><span class="mi">0</span><span class="p">:</span><span class="mi">3</span><span class="p">,</span> <span class="mi">3</span><span class="p">]</span> <span class="o">=</span> <span class="p">[</span><span class="bp">self</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span><span class="p">]</span>
        <span class="k">return</span> <span class="n">ret</span></div>

<div class="viewcode-block" id="SE3Pose.translation_norm"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE3Pose.translation_norm">[docs]</a>    <span class="k">def</span> <span class="nf">translation_norm</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Calculates the Euclidean norm (magnitude) of the translation component pose.&quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="n">math</span><span class="o">.</span><span class="n">sqrt</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">x</span><span class="o">**</span><span class="mi">2</span> <span class="o">+</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span><span class="o">**</span><span class="mi">2</span> <span class="o">+</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span><span class="o">**</span><span class="mi">2</span><span class="p">)</span></div>

<div class="viewcode-block" id="SE3Pose.mult"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE3Pose.mult">[docs]</a>    <span class="k">def</span> <span class="nf">mult</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">se3pose</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Computes the multiplication between the current math_helpers.SE3Pose and the input se3pose.</span>

<span class="sd">        For example, if the &#39;self&#39; SE3Pose represents a_tform_b and the input se3pose represents b_tform_c,</span>
<span class="sd">        then the output will represent the transform a_tform_c.</span>

<span class="sd">        Inputs:</span>
<span class="sd">            se3pose (math_helpers.SE3Pose)</span>

<span class="sd">        Returns:</span>
<span class="sd">            math_helpers.SE3Pose representing the multiplication of two SE(3) poses.</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="n">z</span><span class="p">)</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">rot</span><span class="o">.</span><span class="n">transform_point</span><span class="p">(</span><span class="n">se3pose</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="n">se3pose</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="n">se3pose</span><span class="o">.</span><span class="n">z</span><span class="p">)</span>
        <span class="k">return</span> <span class="n">SE3Pose</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">+</span> <span class="n">x</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">+</span> <span class="n">y</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span> <span class="o">+</span> <span class="n">z</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">rot</span><span class="o">.</span><span class="n">mult</span><span class="p">(</span><span class="n">se3pose</span><span class="o">.</span><span class="n">rot</span><span class="p">))</span></div>

    <span class="k">def</span> <span class="fm">__mul__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">other</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Overrides the &#39;*&#39; symbol to compute the multiplication between two SE(3) poses,</span>
<span class="sd">        or between an SE(3) pose and a Vec3.&quot;&quot;&quot;</span>
        <span class="k">if</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">other</span><span class="p">,</span> <span class="n">Vec3</span><span class="p">):</span>
            <span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="n">z</span><span class="p">)</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">transform_point</span><span class="p">(</span><span class="n">other</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="n">other</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="n">other</span><span class="o">.</span><span class="n">z</span><span class="p">)</span>
            <span class="k">return</span> <span class="n">Vec3</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="n">z</span><span class="p">)</span>
        <span class="k">if</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">other</span><span class="p">,</span> <span class="n">SE3Pose</span><span class="p">):</span>
            <span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="n">z</span><span class="p">)</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">rot</span><span class="o">.</span><span class="n">transform_point</span><span class="p">(</span><span class="n">other</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="n">other</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="n">other</span><span class="o">.</span><span class="n">z</span><span class="p">)</span>
            <span class="k">return</span> <span class="n">SE3Pose</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">+</span> <span class="n">x</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">+</span> <span class="n">y</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span> <span class="o">+</span> <span class="n">z</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">rot</span><span class="o">.</span><span class="n">mult</span><span class="p">(</span><span class="n">other</span><span class="o">.</span><span class="n">rot</span><span class="p">))</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="k">raise</span> <span class="ne">TypeError</span><span class="p">(</span><span class="s2">&quot;Can&#39;t multiply types </span><span class="si">%s</span><span class="s2"> and </span><span class="si">%s</span><span class="s2">.&quot;</span> <span class="o">%</span> <span class="p">(</span><span class="nb">type</span><span class="p">(</span><span class="bp">self</span><span class="p">),</span> <span class="nb">type</span><span class="p">(</span><span class="n">other</span><span class="p">)))</span>

    <span class="nd">@property</span>
    <span class="k">def</span> <span class="nf">position</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Property to allow attribute access of the protobuf message field &#39;position&#39; similar to the geometry_pb2.SE3Pose</span>
<span class="sd">           for the math_helper.SE3Pose.&quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="n">geometry_pb2</span><span class="o">.</span><span class="n">Vec3</span><span class="p">(</span><span class="n">x</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="n">z</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">z</span><span class="p">)</span>


    <span class="nd">@property</span>
    <span class="k">def</span> <span class="nf">rotation</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Property to allow attribute access of the protobuf message field &#39;rotation&#39; similar to the geometry_pb2.SE3Pose</span>
<span class="sd">           for the math_helper.SE3Pose.&quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="n">rot</span>

<div class="viewcode-block" id="SE3Pose.from_matrix"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE3Pose.from_matrix">[docs]</a>    <span class="nd">@staticmethod</span>
    <span class="k">def</span> <span class="nf">from_matrix</span><span class="p">(</span><span class="n">mat</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Extract math_helpers.SE3Pose from a 4x4 matrix.&quot;&quot;&quot;</span>
        <span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="n">z</span> <span class="o">=</span> <span class="n">mat</span><span class="p">[</span><span class="mi">0</span><span class="p">:</span><span class="mi">3</span><span class="p">,</span> <span class="mi">3</span><span class="p">]</span>
        <span class="n">rot</span> <span class="o">=</span> <span class="n">Quat</span><span class="o">.</span><span class="n">from_matrix</span><span class="p">(</span><span class="n">mat</span><span class="p">[</span><span class="mi">0</span><span class="p">:</span><span class="mi">3</span><span class="p">,</span> <span class="mi">0</span><span class="p">:</span><span class="mi">3</span><span class="p">])</span>
        <span class="k">return</span> <span class="n">SE3Pose</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="n">z</span><span class="p">,</span> <span class="n">rot</span><span class="p">)</span></div>


<div class="viewcode-block" id="SE3Pose.from_identity"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE3Pose.from_identity">[docs]</a>    <span class="nd">@staticmethod</span>
    <span class="k">def</span> <span class="nf">from_identity</span><span class="p">():</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Create a math_helpers.SE3Pose representing the identity SE(3) pose.&quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="n">SE3Pose</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="n">Quat</span><span class="p">())</span></div>

<div class="viewcode-block" id="SE3Pose.get_translation"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE3Pose.get_translation">[docs]</a>    <span class="k">def</span> <span class="nf">get_translation</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Returns a 3x1 numpy array representing the translation only of the current SE3Pose.&quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="n">numpy</span><span class="o">.</span><span class="n">array</span><span class="p">([</span><span class="bp">self</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span><span class="p">])</span></div>

<div class="viewcode-block" id="SE3Pose.to_adjoint_matrix"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE3Pose.to_adjoint_matrix">[docs]</a>    <span class="k">def</span> <span class="nf">to_adjoint_matrix</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;This creates the adjoint matrix for the current SE3Pose.</span>

<span class="sd">        The adjoint matrix can be used to change reference frames for a SE(3) velocity vector. For</span>
<span class="sd">        example, if you have math_helpers.SE3Velocity velocity_in_frame_b, then the adjoint matrix</span>
<span class="sd">        for the math_helpers.SE3Pose (representing a_tform_b) can be used as follows to transform</span>
<span class="sd">        the velocity: velocity_in_frame_a = a_tform_b.to_adjoint_matrix() * velocity_in_frame_b</span>

<span class="sd">        Returns:</span>
<span class="sd">            a_adjoint_b (Numpy 6x6 matrix) representing the adjoint matrix for the SE3Pose.</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="n">a_R_b</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">rot</span><span class="o">.</span><span class="n">to_matrix</span><span class="p">()</span>
        <span class="n">position_skew_mat</span> <span class="o">=</span> <span class="n">skew_matrix_3d</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">position</span><span class="p">)</span>
        <span class="n">mat</span> <span class="o">=</span> <span class="n">numpy</span><span class="o">.</span><span class="n">matmul</span><span class="p">(</span><span class="n">position_skew_mat</span><span class="p">,</span> <span class="n">a_R_b</span><span class="p">)</span>
        <span class="n">a_adjoint_b</span> <span class="o">=</span> <span class="n">numpy</span><span class="o">.</span><span class="n">block</span><span class="p">([[</span><span class="n">a_R_b</span><span class="p">,</span> <span class="n">mat</span><span class="p">],</span> <span class="p">[</span><span class="n">numpy</span><span class="o">.</span><span class="n">zeros</span><span class="p">((</span><span class="mi">3</span><span class="p">,</span> <span class="mi">3</span><span class="p">)),</span> <span class="n">a_R_b</span><span class="p">]])</span>
        <span class="k">return</span> <span class="n">a_adjoint_b</span></div>

<div class="viewcode-block" id="SE3Pose.get_closest_se2_transform"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE3Pose.get_closest_se2_transform">[docs]</a>    <span class="k">def</span> <span class="nf">get_closest_se2_transform</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Compute the closest math_helpers.SE2Pose from the current math_helpers.SE3Pose.&quot;&quot;&quot;</span>
        <span class="c1"># For a transform a_T_b, the pose_frame_name represents &quot;frame A&quot;. This must be a gravity</span>
        <span class="c1"># aligned frame, either &quot;vision&quot;, &quot;odom&quot;, or &quot;flat_body&quot; to be safely converted from</span>
        <span class="c1"># an SE3Pose to an SE2Pose with minimal loss of height information.</span>
        <span class="k">return</span> <span class="n">SE2Pose</span><span class="o">.</span><span class="n">flatten</span><span class="p">(</span><span class="bp">self</span><span class="p">)</span></div>

<div class="viewcode-block" id="SE3Pose.interp"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.SE3Pose.interp">[docs]</a>    <span class="nd">@staticmethod</span>
    <span class="k">def</span> <span class="nf">interp</span><span class="p">(</span><span class="n">a</span><span class="p">,</span> <span class="n">b</span><span class="p">,</span> <span class="n">fraction</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Performs a blend of two SE3Poses.  Out = a * (1 - fraction) + b * fraction</span>

<span class="sd">        Args:</span>
<span class="sd">            a(SE3Pose): Lower blend input.</span>
<span class="sd">            b(SE3Pose): Upper blend input.</span>
<span class="sd">            fraction(float): The blending factor.  Should be inside [0, 1]</span>
<span class="sd">        Returns:</span>
<span class="sd">            SE3Pose</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="n">x</span> <span class="o">=</span> <span class="n">a</span><span class="o">.</span><span class="n">x</span> <span class="o">*</span> <span class="p">(</span><span class="mf">1.0</span> <span class="o">-</span> <span class="n">fraction</span><span class="p">)</span> <span class="o">+</span> <span class="n">b</span><span class="o">.</span><span class="n">x</span> <span class="o">*</span> <span class="n">fraction</span>
        <span class="n">y</span> <span class="o">=</span> <span class="n">a</span><span class="o">.</span><span class="n">y</span> <span class="o">*</span> <span class="p">(</span><span class="mf">1.0</span> <span class="o">-</span> <span class="n">fraction</span><span class="p">)</span> <span class="o">+</span> <span class="n">b</span><span class="o">.</span><span class="n">y</span> <span class="o">*</span> <span class="n">fraction</span>
        <span class="n">z</span> <span class="o">=</span> <span class="n">a</span><span class="o">.</span><span class="n">z</span> <span class="o">*</span> <span class="p">(</span><span class="mf">1.0</span> <span class="o">-</span> <span class="n">fraction</span><span class="p">)</span> <span class="o">+</span> <span class="n">b</span><span class="o">.</span><span class="n">z</span> <span class="o">*</span> <span class="n">fraction</span>
        <span class="n">rot</span> <span class="o">=</span> <span class="n">Quat</span><span class="o">.</span><span class="n">slerp</span><span class="p">(</span><span class="n">a</span><span class="o">.</span><span class="n">rot</span><span class="p">,</span> <span class="n">b</span><span class="o">.</span><span class="n">rot</span><span class="p">,</span> <span class="n">fraction</span><span class="p">)</span>
        <span class="k">return</span> <span class="n">SE3Pose</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="n">z</span><span class="p">,</span> <span class="n">rot</span><span class="p">)</span></div></div>


<div class="viewcode-block" id="Quat"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.Quat">[docs]</a><span class="k">class</span> <span class="nc">Quat</span><span class="p">(</span><span class="nb">object</span><span class="p">):</span>
<span class="w">    </span><span class="sd">&quot;&quot;&quot;Class representing a Quaternion.&quot;&quot;&quot;</span>

    <span class="k">def</span> <span class="fm">__init__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">w</span><span class="o">=</span><span class="mi">1</span><span class="p">,</span> <span class="n">x</span><span class="o">=</span><span class="mi">0</span><span class="p">,</span> <span class="n">y</span><span class="o">=</span><span class="mi">0</span><span class="p">,</span> <span class="n">z</span><span class="o">=</span><span class="mi">0</span><span class="p">):</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">w</span> <span class="o">=</span> <span class="n">w</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">=</span> <span class="n">x</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">=</span> <span class="n">y</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">z</span> <span class="o">=</span> <span class="n">z</span>

    <span class="k">def</span> <span class="fm">__repr__</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
        <span class="k">return</span> <span class="s1">&#39;W: </span><span class="si">%0.4f</span><span class="s1"> X: </span><span class="si">%0.4f</span><span class="s1"> Y: </span><span class="si">%0.4f</span><span class="s1"> Z: </span><span class="si">%0.4f</span><span class="s1">&#39;</span> <span class="o">%</span> <span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">w</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span><span class="p">)</span>

<div class="viewcode-block" id="Quat.inverse"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.Quat.inverse">[docs]</a>    <span class="k">def</span> <span class="nf">inverse</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Computes the inverse of the current math_helpers.Quat.&quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="n">Quat</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">w</span><span class="p">,</span> <span class="o">-</span><span class="bp">self</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="o">-</span><span class="bp">self</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="o">-</span><span class="bp">self</span><span class="o">.</span><span class="n">z</span><span class="p">)</span></div>

<div class="viewcode-block" id="Quat.transform_point"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.Quat.transform_point">[docs]</a>    <span class="k">def</span> <span class="nf">transform_point</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="n">z</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Computes the transformation (rotation by the quaternion) of a single (x,y,z)</span>
<span class="sd">            point using the current math_helpers.Quat.&quot;&quot;&quot;</span>
        <span class="n">inv</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">inverse</span><span class="p">()</span>
        <span class="n">q</span> <span class="o">=</span> <span class="n">Quat</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span> <span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="n">z</span><span class="p">)</span>
        <span class="n">q</span> <span class="o">=</span> <span class="n">q</span><span class="o">.</span><span class="n">mult</span><span class="p">(</span><span class="n">inv</span><span class="p">)</span>
        <span class="n">q</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">mult</span><span class="p">(</span><span class="n">q</span><span class="p">)</span>
        <span class="k">return</span> <span class="p">(</span><span class="n">q</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="n">q</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="n">q</span><span class="o">.</span><span class="n">z</span><span class="p">)</span></div>

<div class="viewcode-block" id="Quat.transform_vec3"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.Quat.transform_vec3">[docs]</a>    <span class="k">def</span> <span class="nf">transform_vec3</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">vec3</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Computes the transformation (rotation by the quaternion) of a Vec3</span>
<span class="sd">            point using the current math_helpers.Quat.&quot;&quot;&quot;</span>
        <span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="n">z</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">transform_point</span><span class="p">(</span><span class="n">vec3</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="n">vec3</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="n">vec3</span><span class="o">.</span><span class="n">z</span><span class="p">)</span>
        <span class="k">return</span> <span class="n">geometry_pb2</span><span class="o">.</span><span class="n">Vec3</span><span class="p">(</span><span class="n">x</span><span class="o">=</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="o">=</span><span class="n">y</span><span class="p">,</span> <span class="n">z</span><span class="o">=</span><span class="n">z</span><span class="p">)</span></div>

<div class="viewcode-block" id="Quat.to_matrix"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.Quat.to_matrix">[docs]</a>    <span class="k">def</span> <span class="nf">to_matrix</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Creates the 3x3 numpy rotation matrix from the current math_helpers.Quat&quot;&quot;&quot;</span>
        <span class="n">ret</span> <span class="o">=</span> <span class="n">numpy</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">3</span><span class="p">)</span>
        <span class="n">ret</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="mf">1.0</span> <span class="o">-</span> <span class="mf">2.0</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">-</span> <span class="mf">2.0</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span>
        <span class="n">ret</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">]</span> <span class="o">=</span> <span class="mf">2.0</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">-</span> <span class="mf">2.0</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">w</span>
        <span class="n">ret</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">2</span><span class="p">]</span> <span class="o">=</span> <span class="mf">2.0</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span> <span class="o">+</span> <span class="mf">2.0</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">w</span>

        <span class="n">ret</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="mf">2.0</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">+</span> <span class="mf">2.0</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">w</span>
        <span class="n">ret</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="p">]</span> <span class="o">=</span> <span class="mf">1.0</span> <span class="o">-</span> <span class="mf">2.0</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">-</span> <span class="mf">2.0</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span>
        <span class="n">ret</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">2</span><span class="p">]</span> <span class="o">=</span> <span class="mf">2.0</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span> <span class="o">-</span> <span class="mf">2.0</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">w</span>

        <span class="n">ret</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="mf">2.0</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span> <span class="o">-</span> <span class="mf">2.0</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">w</span>
        <span class="n">ret</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">1</span><span class="p">]</span> <span class="o">=</span> <span class="mf">2.0</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span> <span class="o">+</span> <span class="mf">2.0</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">w</span>
        <span class="n">ret</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">2</span><span class="p">]</span> <span class="o">=</span> <span class="mf">1.0</span> <span class="o">-</span> <span class="mf">2.0</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">-</span> <span class="mf">2.0</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span>

        <span class="k">return</span> <span class="n">ret</span></div>

<div class="viewcode-block" id="Quat.from_matrix"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.Quat.from_matrix">[docs]</a>    <span class="nd">@staticmethod</span>
    <span class="k">def</span> <span class="nf">from_matrix</span><span class="p">(</span><span class="n">rot</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Creates a math_helpers.Quat from a numpy 3x3 matrix representing rotation.&quot;&quot;&quot;</span>
        <span class="n">wt</span> <span class="o">=</span> <span class="mi">1</span> <span class="o">+</span> <span class="n">rot</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">]</span> <span class="o">+</span> <span class="n">rot</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="p">]</span> <span class="o">+</span> <span class="n">rot</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">2</span><span class="p">]</span>
        <span class="c1"># do this most often so we get consistently signed quaternions</span>
        <span class="k">if</span> <span class="n">wt</span> <span class="o">&gt;</span> <span class="mf">0.1</span><span class="p">:</span>
            <span class="k">return</span> <span class="n">Quat</span><span class="o">.</span><span class="n">_from_matrix_w</span><span class="p">(</span><span class="n">rot</span><span class="p">)</span>
        <span class="n">xt</span> <span class="o">=</span> <span class="mi">1</span> <span class="o">+</span> <span class="n">rot</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">]</span> <span class="o">-</span> <span class="n">rot</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="p">]</span> <span class="o">-</span> <span class="n">rot</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">2</span><span class="p">]</span>
        <span class="n">yt</span> <span class="o">=</span> <span class="mi">1</span> <span class="o">-</span> <span class="n">rot</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">]</span> <span class="o">+</span> <span class="n">rot</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="p">]</span> <span class="o">-</span> <span class="n">rot</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">2</span><span class="p">]</span>
        <span class="n">zt</span> <span class="o">=</span> <span class="mi">1</span> <span class="o">-</span> <span class="n">rot</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">]</span> <span class="o">-</span> <span class="n">rot</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="p">]</span> <span class="o">+</span> <span class="n">rot</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">2</span><span class="p">]</span>
        <span class="c1"># This is a list of (function that divides by the square root of val, val)</span>
        <span class="c1"># The idea is to find the largest of wt, xt, yt, zt and then use the corresponding</span>
        <span class="c1"># function to do the conversion.  This will do the best thing numerically.</span>
        <span class="n">t</span> <span class="o">=</span> <span class="p">[(</span><span class="n">Quat</span><span class="o">.</span><span class="n">_from_matrix_w</span><span class="p">,</span> <span class="n">wt</span><span class="p">),</span> <span class="p">(</span><span class="n">Quat</span><span class="o">.</span><span class="n">_from_matrix_x</span><span class="p">,</span> <span class="n">xt</span><span class="p">),</span> <span class="p">(</span><span class="n">Quat</span><span class="o">.</span><span class="n">_from_matrix_y</span><span class="p">,</span> <span class="n">yt</span><span class="p">),</span>
             <span class="p">(</span><span class="n">Quat</span><span class="o">.</span><span class="n">_from_matrix_z</span><span class="p">,</span> <span class="n">zt</span><span class="p">)]</span>
        <span class="n">from_matrix_coord</span><span class="p">,</span> <span class="n">val</span> <span class="o">=</span> <span class="nb">max</span><span class="p">(</span><span class="n">t</span><span class="p">,</span> <span class="n">key</span><span class="o">=</span><span class="k">lambda</span> <span class="n">entry</span><span class="p">:</span> <span class="n">entry</span><span class="p">[</span><span class="mi">1</span><span class="p">])</span>
        <span class="k">if</span> <span class="n">val</span> <span class="o">&lt;</span> <span class="mf">1e-6</span><span class="p">:</span>
            <span class="c1"># This shouldn&#39;t actually be theoretically possible.</span>
            <span class="k">raise</span> <span class="ne">ArithmeticError</span><span class="p">(</span>
                <span class="s1">&#39;Matrix cannot be converged to quaternion.  Are you sure this is a valid&#39;</span>
                <span class="s1">&#39; rotation matrix?</span><span class="se">\n</span><span class="s1">&#39;</span> <span class="o">+</span> <span class="nb">str</span><span class="p">(</span><span class="n">rot</span><span class="p">))</span>
        <span class="k">return</span> <span class="n">from_matrix_coord</span><span class="p">(</span><span class="n">rot</span><span class="p">)</span></div>

    <span class="nd">@staticmethod</span>
    <span class="k">def</span> <span class="nf">_from_matrix_w</span><span class="p">(</span><span class="n">rot</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Computes the value of the quaternion for the w-axis from a numpy 3x3 rotation matrix.&quot;&quot;&quot;</span>
        <span class="n">w</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">sqrt</span><span class="p">(</span><span class="mi">1</span> <span class="o">+</span> <span class="n">rot</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">]</span> <span class="o">+</span> <span class="n">rot</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="p">]</span> <span class="o">+</span> <span class="n">rot</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">2</span><span class="p">])</span> <span class="o">*</span> <span class="mf">0.5</span>
        <span class="k">return</span> <span class="n">Quat</span><span class="p">(</span><span class="n">w</span><span class="o">=</span><span class="n">w</span><span class="p">,</span> <span class="n">x</span><span class="o">=</span><span class="p">(</span><span class="n">rot</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">1</span><span class="p">]</span> <span class="o">-</span> <span class="n">rot</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">2</span><span class="p">])</span> <span class="o">/</span> <span class="p">(</span><span class="mf">4.0</span> <span class="o">*</span> <span class="n">w</span><span class="p">),</span>
                    <span class="n">y</span><span class="o">=</span><span class="p">(</span><span class="n">rot</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">2</span><span class="p">]</span> <span class="o">-</span> <span class="n">rot</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">0</span><span class="p">])</span> <span class="o">/</span> <span class="p">(</span><span class="mf">4.0</span> <span class="o">*</span> <span class="n">w</span><span class="p">),</span> <span class="n">z</span><span class="o">=</span><span class="p">(</span><span class="n">rot</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">0</span><span class="p">]</span> <span class="o">-</span> <span class="n">rot</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">])</span> <span class="o">/</span> <span class="p">(</span><span class="mf">4.0</span> <span class="o">*</span> <span class="n">w</span><span class="p">))</span>

    <span class="nd">@staticmethod</span>
    <span class="k">def</span> <span class="nf">_from_matrix_x</span><span class="p">(</span><span class="n">rot</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Computes the value of the quaternion for the x-axis from a numpy 3x3 rotation matrix.&quot;&quot;&quot;</span>
        <span class="n">x</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">sqrt</span><span class="p">(</span><span class="mi">1</span> <span class="o">+</span> <span class="n">rot</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">]</span> <span class="o">-</span> <span class="n">rot</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="p">]</span> <span class="o">-</span> <span class="n">rot</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">2</span><span class="p">])</span> <span class="o">*</span> <span class="mf">0.5</span>
        <span class="k">return</span> <span class="n">Quat</span><span class="p">(</span><span class="n">w</span><span class="o">=</span><span class="p">(</span><span class="n">rot</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">1</span><span class="p">]</span> <span class="o">-</span> <span class="n">rot</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">2</span><span class="p">])</span> <span class="o">/</span> <span class="p">(</span><span class="mf">4.0</span> <span class="o">*</span> <span class="n">x</span><span class="p">),</span> <span class="n">x</span><span class="o">=</span><span class="n">x</span><span class="p">,</span>
                    <span class="n">y</span><span class="o">=</span><span class="p">(</span><span class="n">rot</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">]</span> <span class="o">+</span> <span class="n">rot</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">0</span><span class="p">])</span> <span class="o">/</span> <span class="p">(</span><span class="mf">4.0</span> <span class="o">*</span> <span class="n">x</span><span class="p">),</span> <span class="n">z</span><span class="o">=</span><span class="p">(</span><span class="n">rot</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">2</span><span class="p">]</span> <span class="o">+</span> <span class="n">rot</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">0</span><span class="p">])</span> <span class="o">/</span> <span class="p">(</span><span class="mf">4.0</span> <span class="o">*</span> <span class="n">x</span><span class="p">))</span>

    <span class="nd">@staticmethod</span>
    <span class="k">def</span> <span class="nf">_from_matrix_y</span><span class="p">(</span><span class="n">rot</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Computes the value of the quaternion for the y-axis from a numpy 3x3 rotation matrix.&quot;&quot;&quot;</span>
        <span class="n">y</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">sqrt</span><span class="p">(</span><span class="mi">1</span> <span class="o">-</span> <span class="n">rot</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">]</span> <span class="o">+</span> <span class="n">rot</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="p">]</span> <span class="o">-</span> <span class="n">rot</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">2</span><span class="p">])</span> <span class="o">*</span> <span class="mf">0.5</span>
        <span class="k">return</span> <span class="n">Quat</span><span class="p">(</span><span class="n">w</span><span class="o">=</span><span class="p">(</span><span class="n">rot</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">2</span><span class="p">]</span> <span class="o">-</span> <span class="n">rot</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">0</span><span class="p">])</span> <span class="o">/</span> <span class="p">(</span><span class="mf">4.0</span> <span class="o">*</span> <span class="n">y</span><span class="p">),</span> <span class="n">x</span><span class="o">=</span><span class="p">(</span><span class="n">rot</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">]</span> <span class="o">+</span> <span class="n">rot</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">0</span><span class="p">])</span> <span class="o">/</span> <span class="p">(</span><span class="mf">4.0</span> <span class="o">*</span> <span class="n">y</span><span class="p">),</span>
                    <span class="n">y</span><span class="o">=</span><span class="n">y</span><span class="p">,</span> <span class="n">z</span><span class="o">=</span><span class="p">(</span><span class="n">rot</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">2</span><span class="p">]</span> <span class="o">+</span> <span class="n">rot</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">1</span><span class="p">])</span> <span class="o">/</span> <span class="p">(</span><span class="mf">4.0</span> <span class="o">*</span> <span class="n">y</span><span class="p">))</span>

    <span class="nd">@staticmethod</span>
    <span class="k">def</span> <span class="nf">_from_matrix_z</span><span class="p">(</span><span class="n">rot</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Computes the value of the quaternion for the z-axis from a numpy 3x3 rotation matrix.&quot;&quot;&quot;</span>
        <span class="n">z</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">sqrt</span><span class="p">(</span><span class="mi">1</span> <span class="o">-</span> <span class="n">rot</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">]</span> <span class="o">-</span> <span class="n">rot</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="p">]</span> <span class="o">+</span> <span class="n">rot</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">2</span><span class="p">])</span> <span class="o">*</span> <span class="mf">0.5</span>
        <span class="k">return</span> <span class="n">Quat</span><span class="p">(</span><span class="n">w</span><span class="o">=</span><span class="p">(</span><span class="n">rot</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">0</span><span class="p">]</span> <span class="o">-</span> <span class="n">rot</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">])</span> <span class="o">/</span> <span class="p">(</span><span class="mf">4.0</span> <span class="o">*</span> <span class="n">z</span><span class="p">),</span> <span class="n">x</span><span class="o">=</span><span class="p">(</span><span class="n">rot</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">2</span><span class="p">]</span> <span class="o">+</span> <span class="n">rot</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">0</span><span class="p">])</span> <span class="o">/</span> <span class="p">(</span><span class="mf">4.0</span> <span class="o">*</span> <span class="n">z</span><span class="p">),</span>
                    <span class="n">y</span><span class="o">=</span><span class="p">(</span><span class="n">rot</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">2</span><span class="p">]</span> <span class="o">+</span> <span class="n">rot</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">1</span><span class="p">])</span> <span class="o">/</span> <span class="p">(</span><span class="mf">4.0</span> <span class="o">*</span> <span class="n">z</span><span class="p">),</span> <span class="n">z</span><span class="o">=</span><span class="n">z</span><span class="p">)</span>

<div class="viewcode-block" id="Quat.from_roll"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.Quat.from_roll">[docs]</a>    <span class="nd">@staticmethod</span>
    <span class="k">def</span> <span class="nf">from_roll</span><span class="p">(</span><span class="n">angle</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Computes a representative math_helpers.Quat from the Euler angle for roll.&quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="n">Quat</span><span class="p">(</span><span class="n">w</span><span class="o">=</span><span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">angle</span> <span class="o">/</span> <span class="mf">2.0</span><span class="p">),</span> <span class="n">x</span><span class="o">=</span><span class="n">math</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">angle</span> <span class="o">/</span> <span class="mf">2.0</span><span class="p">))</span></div>

<div class="viewcode-block" id="Quat.from_pitch"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.Quat.from_pitch">[docs]</a>    <span class="nd">@staticmethod</span>
    <span class="k">def</span> <span class="nf">from_pitch</span><span class="p">(</span><span class="n">angle</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Computes a representative math_helpers.Quat from the Euler angle for pitch.&quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="n">Quat</span><span class="p">(</span><span class="n">w</span><span class="o">=</span><span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">angle</span> <span class="o">/</span> <span class="mf">2.0</span><span class="p">),</span> <span class="n">y</span><span class="o">=</span><span class="n">math</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">angle</span> <span class="o">/</span> <span class="mf">2.0</span><span class="p">))</span></div>

<div class="viewcode-block" id="Quat.from_yaw"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.Quat.from_yaw">[docs]</a>    <span class="nd">@staticmethod</span>
    <span class="k">def</span> <span class="nf">from_yaw</span><span class="p">(</span><span class="n">angle</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Computes a representative math_helpers.Quat from the Euler angle for yaw.&quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="n">Quat</span><span class="p">(</span><span class="n">w</span><span class="o">=</span><span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">angle</span> <span class="o">/</span> <span class="mf">2.0</span><span class="p">),</span> <span class="n">z</span><span class="o">=</span><span class="n">math</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">angle</span> <span class="o">/</span> <span class="mf">2.0</span><span class="p">))</span></div>

<div class="viewcode-block" id="Quat.to_roll"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.Quat.to_roll">[docs]</a>    <span class="k">def</span> <span class="nf">to_roll</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Computes the Euler angle roll from the current math_helpers.Quat&quot;&quot;&quot;</span>
        <span class="k">if</span> <span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">w</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">w</span> <span class="o">+</span> <span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">+</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">+</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span> <span class="o">==</span> <span class="mf">0.0</span><span class="p">):</span>
            <span class="k">return</span> <span class="mf">0.0</span>
        <span class="n">t0</span> <span class="o">=</span> <span class="mf">2.0</span> <span class="o">*</span> <span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">w</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">+</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span><span class="p">)</span>
        <span class="n">t1</span> <span class="o">=</span> <span class="mf">1.0</span> <span class="o">-</span> <span class="mf">2.0</span> <span class="o">*</span> <span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">+</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span><span class="p">)</span>
        <span class="k">return</span> <span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="n">t0</span><span class="p">,</span> <span class="n">t1</span><span class="p">)</span></div>

<div class="viewcode-block" id="Quat.to_pitch"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.Quat.to_pitch">[docs]</a>    <span class="k">def</span> <span class="nf">to_pitch</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Computes the Euler angle pitch from the current math_helpers.Quat&quot;&quot;&quot;</span>
        <span class="k">if</span> <span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">w</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">w</span> <span class="o">+</span> <span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">+</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">+</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span> <span class="o">==</span> <span class="mf">0.0</span><span class="p">):</span>
            <span class="k">return</span> <span class="mf">0.0</span>
        <span class="n">t2</span> <span class="o">=</span> <span class="mf">2.0</span> <span class="o">*</span> <span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">w</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">-</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">x</span><span class="p">)</span>
        <span class="k">if</span> <span class="n">t2</span> <span class="o">&lt;</span> <span class="o">-</span><span class="mf">1.0</span><span class="p">:</span>
            <span class="n">t2</span> <span class="o">=</span> <span class="o">-</span><span class="mf">1.0</span>
        <span class="k">if</span> <span class="n">t2</span> <span class="o">&gt;</span> <span class="mf">1.0</span><span class="p">:</span>
            <span class="n">t2</span> <span class="o">=</span> <span class="mf">1.0</span>
        <span class="k">return</span> <span class="n">math</span><span class="o">.</span><span class="n">asin</span><span class="p">(</span><span class="n">t2</span><span class="p">)</span></div>

<div class="viewcode-block" id="Quat.to_yaw"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.Quat.to_yaw">[docs]</a>    <span class="k">def</span> <span class="nf">to_yaw</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Computes the Euler angle yaw from the current math_helpers.Quat&quot;&quot;&quot;</span>
        <span class="n">yaw_only_quat</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">closest_yaw_only_quaternion</span><span class="p">()</span>
        <span class="k">return</span> <span class="n">recenter_angle_mod</span><span class="p">(</span><span class="mi">2</span> <span class="o">*</span> <span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="n">yaw_only_quat</span><span class="o">.</span><span class="n">z</span><span class="p">,</span> <span class="n">yaw_only_quat</span><span class="o">.</span><span class="n">w</span><span class="p">),</span> <span class="mf">0.0</span><span class="p">)</span></div>

<div class="viewcode-block" id="Quat.to_axis_angle"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.Quat.to_axis_angle">[docs]</a>    <span class="k">def</span> <span class="nf">to_axis_angle</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Computes the angle and the respective axis from the math_helpers.Quat&quot;&quot;&quot;</span>
        <span class="k">if</span> <span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">w</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">w</span> <span class="o">+</span> <span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">+</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">+</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span> <span class="o">==</span> <span class="mf">0.0</span><span class="p">):</span>
            <span class="k">return</span> <span class="p">(</span><span class="mf">0.0</span><span class="p">,</span> <span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">])</span>

        <span class="n">mag</span> <span class="o">=</span> <span class="mf">1.0</span> <span class="o">-</span> <span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">w</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">w</span><span class="p">)</span>
        <span class="k">if</span> <span class="n">mag</span> <span class="o">&lt;=</span> <span class="mf">1e-12</span><span class="p">:</span>
            <span class="k">return</span> <span class="p">(</span><span class="mf">0.0</span><span class="p">,</span> <span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">])</span>

        <span class="n">denom</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">sqrt</span><span class="p">(</span><span class="n">mag</span><span class="p">)</span>
        <span class="k">if</span> <span class="n">denom</span> <span class="o">&lt;</span> <span class="mf">1e-12</span><span class="p">:</span>
            <span class="k">return</span> <span class="p">(</span><span class="mf">0.0</span><span class="p">,</span> <span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">])</span>

        <span class="n">angle</span> <span class="o">=</span> <span class="mf">2.0</span> <span class="o">*</span> <span class="n">math</span><span class="o">.</span><span class="n">acos</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">w</span><span class="p">)</span>
        <span class="n">axis</span> <span class="o">=</span> <span class="p">[</span><span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">/</span> <span class="n">denom</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">/</span> <span class="n">denom</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span> <span class="o">/</span> <span class="n">denom</span><span class="p">]</span>
        <span class="k">return</span> <span class="p">(</span><span class="n">angle</span><span class="p">,</span> <span class="n">axis</span><span class="p">)</span></div>

<div class="viewcode-block" id="Quat.from_obj"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.Quat.from_obj">[docs]</a>    <span class="nd">@staticmethod</span>
    <span class="nd">@deprecated</span><span class="p">(</span><span class="n">reason</span><span class="o">=</span><span class="s1">&#39;Use from_proto instead.&#39;</span><span class="p">,</span> <span class="n">version</span><span class="o">=</span><span class="s1">&#39;3.1.0&#39;</span><span class="p">)</span>
    <span class="k">def</span> <span class="nf">from_obj</span><span class="p">(</span><span class="n">proto</span><span class="p">):</span>
        <span class="k">return</span> <span class="n">Quat</span><span class="o">.</span><span class="n">from_proto</span><span class="p">(</span><span class="n">proto</span><span class="p">)</span></div>

<div class="viewcode-block" id="Quat.from_proto"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.Quat.from_proto">[docs]</a>    <span class="nd">@staticmethod</span>
    <span class="k">def</span> <span class="nf">from_proto</span><span class="p">(</span><span class="n">proto</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Create a math_helpers.Quat from a geometry_pb2.Quaternion proto.&quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="n">Quat</span><span class="p">(</span><span class="n">proto</span><span class="o">.</span><span class="n">w</span><span class="p">,</span> <span class="n">proto</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="n">proto</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="n">proto</span><span class="o">.</span><span class="n">z</span><span class="p">)</span></div>

<div class="viewcode-block" id="Quat.to_obj"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.Quat.to_obj">[docs]</a>    <span class="k">def</span> <span class="nf">to_obj</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">proto</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Adds the math_helpers.Quat properties into the geometry_pb2.Quaternion &#39;proto&#39;.&quot;&quot;&quot;</span>
        <span class="n">proto</span><span class="o">.</span><span class="n">w</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">w</span>
        <span class="n">proto</span><span class="o">.</span><span class="n">x</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">x</span>
        <span class="n">proto</span><span class="o">.</span><span class="n">y</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span>
        <span class="n">proto</span><span class="o">.</span><span class="n">z</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span></div>

<div class="viewcode-block" id="Quat.to_proto"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.Quat.to_proto">[docs]</a>    <span class="k">def</span> <span class="nf">to_proto</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Converts the math_helpers.Quat into an output of the protobuf geometry_pb2.Quaternion.&quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="n">geometry_pb2</span><span class="o">.</span><span class="n">Quaternion</span><span class="p">(</span><span class="n">w</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">w</span><span class="p">,</span> <span class="n">x</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="n">z</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">z</span><span class="p">)</span></div>

<div class="viewcode-block" id="Quat.mult"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.Quat.mult">[docs]</a>    <span class="k">def</span> <span class="nf">mult</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">other_quat</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Computes the multiplication of two math_helpers.Quats.&quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="n">Quat</span><span class="p">(</span>
            <span class="bp">self</span><span class="o">.</span><span class="n">w</span> <span class="o">*</span> <span class="n">other_quat</span><span class="o">.</span><span class="n">w</span> <span class="o">-</span> <span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">*</span> <span class="n">other_quat</span><span class="o">.</span><span class="n">x</span> <span class="o">-</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">*</span> <span class="n">other_quat</span><span class="o">.</span><span class="n">y</span> <span class="o">-</span>
            <span class="bp">self</span><span class="o">.</span><span class="n">z</span> <span class="o">*</span> <span class="n">other_quat</span><span class="o">.</span><span class="n">z</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">w</span> <span class="o">*</span> <span class="n">other_quat</span><span class="o">.</span><span class="n">x</span> <span class="o">+</span> <span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">*</span> <span class="n">other_quat</span><span class="o">.</span><span class="n">w</span> <span class="o">+</span>
            <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">*</span> <span class="n">other_quat</span><span class="o">.</span><span class="n">z</span> <span class="o">-</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span> <span class="o">*</span> <span class="n">other_quat</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">w</span> <span class="o">*</span> <span class="n">other_quat</span><span class="o">.</span><span class="n">y</span> <span class="o">-</span>
            <span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">*</span> <span class="n">other_quat</span><span class="o">.</span><span class="n">z</span> <span class="o">+</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">*</span> <span class="n">other_quat</span><span class="o">.</span><span class="n">w</span> <span class="o">+</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span> <span class="o">*</span> <span class="n">other_quat</span><span class="o">.</span><span class="n">x</span><span class="p">,</span>
            <span class="bp">self</span><span class="o">.</span><span class="n">w</span> <span class="o">*</span> <span class="n">other_quat</span><span class="o">.</span><span class="n">z</span> <span class="o">+</span> <span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">*</span> <span class="n">other_quat</span><span class="o">.</span><span class="n">y</span> <span class="o">-</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">*</span> <span class="n">other_quat</span><span class="o">.</span><span class="n">x</span> <span class="o">+</span>
            <span class="bp">self</span><span class="o">.</span><span class="n">z</span> <span class="o">*</span> <span class="n">other_quat</span><span class="o">.</span><span class="n">w</span><span class="p">)</span></div>

    <span class="k">def</span> <span class="fm">__mul__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">other</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Overrides the &#39;*&#39; symbol to compute the multiplication between two math_helpers.Quats</span>
<span class="sd">        or between a Quat and a Vec3.&quot;&quot;&quot;</span>
        <span class="k">if</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">other</span><span class="p">,</span> <span class="n">Vec3</span><span class="p">):</span>
            <span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="n">z</span><span class="p">)</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">transform_point</span><span class="p">(</span><span class="n">other</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="n">other</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="n">other</span><span class="o">.</span><span class="n">z</span><span class="p">)</span>
            <span class="k">return</span> <span class="n">Vec3</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="n">z</span><span class="p">)</span>
        <span class="k">if</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">other</span><span class="p">,</span> <span class="n">Quat</span><span class="p">):</span>
            <span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="n">mult</span><span class="p">(</span><span class="n">other</span><span class="p">)</span>
        <span class="k">raise</span> <span class="ne">TypeError</span><span class="p">(</span><span class="s2">&quot;Can&#39;t multiply types </span><span class="si">%s</span><span class="s2"> and </span><span class="si">%s</span><span class="s2">.&quot;</span> <span class="o">%</span> <span class="p">(</span><span class="nb">type</span><span class="p">(</span><span class="bp">self</span><span class="p">),</span> <span class="nb">type</span><span class="p">(</span><span class="n">other</span><span class="p">)))</span>

<div class="viewcode-block" id="Quat.normalize"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.Quat.normalize">[docs]</a>    <span class="k">def</span> <span class="nf">normalize</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Normalizes the quaternion.&quot;&quot;&quot;</span>
        <span class="n">q</span> <span class="o">=</span> <span class="n">numpy</span><span class="o">.</span><span class="n">array</span><span class="p">([</span><span class="bp">self</span><span class="o">.</span><span class="n">w</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span><span class="p">])</span>
        <span class="nb">len</span> <span class="o">=</span> <span class="n">numpy</span><span class="o">.</span><span class="n">sqrt</span><span class="p">(</span><span class="n">numpy</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">q</span><span class="o">.</span><span class="n">transpose</span><span class="p">(),</span> <span class="n">q</span><span class="p">))</span>
        <span class="k">if</span> <span class="p">(</span><span class="nb">len</span> <span class="o">&lt;</span> <span class="mf">1e-15</span><span class="p">):</span>
            <span class="n">q</span> <span class="o">=</span> <span class="p">[</span><span class="mf">1.0</span><span class="p">,</span> <span class="mf">0.0</span><span class="p">,</span> <span class="mf">0.0</span><span class="p">,</span> <span class="mf">0.0</span><span class="p">]</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="n">q</span> <span class="o">/=</span> <span class="nb">len</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">w</span> <span class="o">=</span> <span class="n">q</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">x</span> <span class="o">=</span> <span class="n">q</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">y</span> <span class="o">=</span> <span class="n">q</span><span class="p">[</span><span class="mi">2</span><span class="p">]</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">z</span> <span class="o">=</span> <span class="n">q</span><span class="p">[</span><span class="mi">3</span><span class="p">]</span>
        <span class="k">return</span> <span class="bp">self</span></div>

<div class="viewcode-block" id="Quat.closest_yaw_only_quaternion"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.Quat.closest_yaw_only_quaternion">[docs]</a>    <span class="k">def</span> <span class="nf">closest_yaw_only_quaternion</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot;Computes a yaw-only math_helpers.Quat from the current roll/pitch/yaw math_helpers.Quat&quot;&quot;&quot;</span>
        <span class="n">mag</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">sqrt</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">w</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">w</span> <span class="o">+</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">z</span><span class="p">)</span>
        <span class="k">if</span> <span class="n">mag</span> <span class="o">&gt;</span> <span class="mi">0</span><span class="p">:</span>
            <span class="k">return</span> <span class="n">Quat</span><span class="p">(</span><span class="n">w</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">w</span> <span class="o">/</span> <span class="n">mag</span><span class="p">,</span> <span class="n">x</span><span class="o">=</span><span class="mi">0</span><span class="p">,</span> <span class="n">y</span><span class="o">=</span><span class="mi">0</span><span class="p">,</span> <span class="n">z</span><span class="o">=</span><span class="bp">self</span><span class="o">.</span><span class="n">z</span> <span class="o">/</span> <span class="n">mag</span><span class="p">)</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="c1"># If the problem is ill posed (i.e. z-axis of quaternion is [0, 0, -1]), then preserve old</span>
            <span class="c1"># behavior and always rotate 180 degrees around the y-axis.</span>
            <span class="k">return</span> <span class="n">Quat</span><span class="p">(</span><span class="n">w</span><span class="o">=</span><span class="mi">0</span><span class="p">,</span> <span class="n">x</span><span class="o">=</span><span class="mi">0</span><span class="p">,</span> <span class="n">y</span><span class="o">=</span><span class="mi">1</span><span class="p">,</span> <span class="n">z</span><span class="o">=</span><span class="mi">0</span><span class="p">)</span> <span class="o">*</span> <span class="bp">self</span></div>

<div class="viewcode-block" id="Quat.slerp"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.Quat.slerp">[docs]</a>    <span class="nd">@staticmethod</span>
    <span class="k">def</span> <span class="nf">slerp</span><span class="p">(</span><span class="n">a</span><span class="p">,</span> <span class="n">b</span><span class="p">,</span> <span class="n">fraction</span><span class="p">):</span>
        <span class="n">v0</span> <span class="o">=</span> <span class="n">numpy</span><span class="o">.</span><span class="n">array</span><span class="p">([</span><span class="n">a</span><span class="o">.</span><span class="n">w</span><span class="p">,</span> <span class="n">a</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="n">a</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="n">a</span><span class="o">.</span><span class="n">z</span><span class="p">])</span>
        <span class="n">v1</span> <span class="o">=</span> <span class="n">numpy</span><span class="o">.</span><span class="n">array</span><span class="p">([</span><span class="n">b</span><span class="o">.</span><span class="n">w</span><span class="p">,</span> <span class="n">b</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="n">b</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="n">b</span><span class="o">.</span><span class="n">z</span><span class="p">])</span>
        <span class="n">dot</span> <span class="o">=</span> <span class="n">numpy</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">v0</span><span class="o">.</span><span class="n">transpose</span><span class="p">(),</span> <span class="n">v1</span><span class="p">)</span>
        <span class="c1"># If the dot product is negative, slerp will not take</span>
        <span class="c1"># the shorter path. Note that v1 and -v1 are equivalent when</span>
        <span class="c1"># the negation is applied to all four components. Fix by</span>
        <span class="c1"># reversing one quaternion.</span>
        <span class="k">if</span> <span class="n">dot</span> <span class="o">&lt;</span> <span class="mf">0.0</span><span class="p">:</span>
            <span class="n">v0</span> <span class="o">*=</span> <span class="o">-</span><span class="mf">1.0</span>
            <span class="n">dot</span> <span class="o">=</span> <span class="o">-</span><span class="n">dot</span>

        <span class="n">DOT_THRESHOLD</span> <span class="o">=</span> <span class="mf">1.0</span> <span class="o">-</span> <span class="mf">1e-4</span>
        <span class="k">if</span> <span class="n">dot</span> <span class="o">&gt;</span> <span class="n">DOT_THRESHOLD</span><span class="p">:</span>
            <span class="c1"># If the inputs are too close for comfort, linearly interpolate</span>
            <span class="c1"># and normalize the result.</span>
            <span class="n">result</span> <span class="o">=</span> <span class="n">v0</span> <span class="o">+</span> <span class="n">fraction</span> <span class="o">*</span> <span class="p">(</span><span class="n">v1</span> <span class="o">-</span> <span class="n">v0</span><span class="p">)</span>
            <span class="n">result</span> <span class="o">/=</span> <span class="n">numpy</span><span class="o">.</span><span class="n">sqrt</span><span class="p">(</span><span class="n">numpy</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">result</span><span class="o">.</span><span class="n">transpose</span><span class="p">(),</span> <span class="n">result</span><span class="p">))</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="c1"># Since dot is in range [0, DOT_THRESHOLD], acos is safe</span>
            <span class="n">theta_0</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">acos</span><span class="p">(</span><span class="n">dot</span><span class="p">)</span>  <span class="c1"># theta_0 = angle between input vectors</span>
            <span class="n">theta</span> <span class="o">=</span> <span class="n">theta_0</span> <span class="o">*</span> <span class="n">fraction</span>  <span class="c1"># theta = angle between v0 and result</span>
            <span class="n">sin_theta</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">theta</span><span class="p">)</span>  <span class="c1"># compute this value only once</span>
            <span class="n">sin_theta_0</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">theta_0</span><span class="p">)</span>  <span class="c1"># compute this value only once</span>

            <span class="n">s0</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span>
                <span class="n">theta</span><span class="p">)</span> <span class="o">-</span> <span class="n">dot</span> <span class="o">*</span> <span class="n">sin_theta</span> <span class="o">/</span> <span class="n">sin_theta_0</span>  <span class="c1"># == sin(theta_0 - theta) / sin(theta_0)</span>
            <span class="n">s1</span> <span class="o">=</span> <span class="n">sin_theta</span> <span class="o">/</span> <span class="n">sin_theta_0</span>

            <span class="n">result</span> <span class="o">=</span> <span class="p">(</span><span class="n">s0</span> <span class="o">*</span> <span class="n">v0</span><span class="p">)</span> <span class="o">+</span> <span class="p">(</span><span class="n">s1</span> <span class="o">*</span> <span class="n">v1</span><span class="p">)</span>
        <span class="k">return</span> <span class="n">Quat</span><span class="p">(</span><span class="n">result</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">result</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">result</span><span class="p">[</span><span class="mi">2</span><span class="p">],</span> <span class="n">result</span><span class="p">[</span><span class="mi">3</span><span class="p">])</span></div>

<div class="viewcode-block" id="Quat.from_two_vectors"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.Quat.from_two_vectors">[docs]</a>    <span class="nd">@staticmethod</span>
    <span class="k">def</span> <span class="nf">from_two_vectors</span><span class="p">(</span><span class="n">u_in</span><span class="p">:</span> <span class="n">Vec3</span><span class="p">,</span> <span class="n">v_in</span><span class="p">:</span> <span class="n">Vec3</span><span class="p">):</span>
<span class="w">        </span><span class="sd">&quot;&quot;&quot; Returns a quaternion representing the rotation from u to v.&quot;&quot;&quot;</span>
        <span class="c1"># Normalizing by max avoids all sorts of underflow and overflow issues when we multiply</span>
        <span class="c1"># terms together, including any issues in calculating the norm itself.</span>
        <span class="n">max_u</span> <span class="o">=</span> <span class="nb">max</span><span class="p">([</span><span class="n">math</span><span class="o">.</span><span class="n">fabs</span><span class="p">(</span><span class="n">u_in</span><span class="p">[</span><span class="mi">0</span><span class="p">]),</span> <span class="n">math</span><span class="o">.</span><span class="n">fabs</span><span class="p">(</span><span class="n">u_in</span><span class="p">[</span><span class="mi">1</span><span class="p">]),</span> <span class="n">math</span><span class="o">.</span><span class="n">fabs</span><span class="p">(</span><span class="n">u_in</span><span class="p">[</span><span class="mi">2</span><span class="p">])])</span>
        <span class="n">max_v</span> <span class="o">=</span> <span class="nb">max</span><span class="p">([</span><span class="n">math</span><span class="o">.</span><span class="n">fabs</span><span class="p">(</span><span class="n">v_in</span><span class="p">[</span><span class="mi">0</span><span class="p">]),</span> <span class="n">math</span><span class="o">.</span><span class="n">fabs</span><span class="p">(</span><span class="n">v_in</span><span class="p">[</span><span class="mi">1</span><span class="p">]),</span> <span class="n">math</span><span class="o">.</span><span class="n">fabs</span><span class="p">(</span><span class="n">v_in</span><span class="p">[</span><span class="mi">2</span><span class="p">])])</span>
        <span class="k">if</span> <span class="n">max_u</span> <span class="o">==</span> <span class="mi">0</span> <span class="ow">or</span> <span class="n">max_v</span> <span class="o">==</span> <span class="mi">0</span><span class="p">:</span>
            <span class="c1"># Undefined; return identity</span>
            <span class="k">return</span> <span class="n">Quat</span><span class="p">(</span><span class="mi">1</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">)</span>
        <span class="n">u</span> <span class="o">=</span> <span class="n">u_in</span> <span class="o">*</span> <span class="p">(</span><span class="mi">1</span> <span class="o">/</span> <span class="n">max_u</span><span class="p">)</span>
        <span class="n">v</span> <span class="o">=</span> <span class="n">v_in</span> <span class="o">*</span> <span class="p">(</span><span class="mi">1</span> <span class="o">/</span> <span class="n">max_v</span><span class="p">)</span>

        <span class="n">u_dot_v</span> <span class="o">=</span> <span class="n">u</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">v</span><span class="p">)</span>
        <span class="n">u_dot_u</span> <span class="o">=</span> <span class="n">u</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">u</span><span class="p">)</span>
        <span class="n">v_dot_v</span> <span class="o">=</span> <span class="n">v</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">v</span><span class="p">)</span>
        <span class="n">norm_u_norm_v</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">sqrt</span><span class="p">(</span><span class="n">u_dot_u</span> <span class="o">*</span> <span class="n">v_dot_v</span><span class="p">)</span>

        <span class="k">if</span> <span class="n">u_dot_v</span> <span class="o">&lt;</span> <span class="mi">0</span><span class="p">:</span>
            <span class="c1">#When this is the case, things get annoying because the |u||v| + u.v (see u_dot_v &gt;= 0</span>
            <span class="c1">#case below) has cancellation; this leads us to a different formula that is sensitive to</span>
            <span class="c1">#the magnitude of c. If the vectors are close to antipodal, the cross product itself can</span>
            <span class="c1">#be ill conditioned. Algebraically, u x (u + v) is equal to u x v, but, the result is</span>
            <span class="c1">#much more likely to be orthogonal to u and v for extreme cases.</span>
            <span class="n">c</span> <span class="o">=</span> <span class="n">u</span><span class="o">.</span><span class="n">cross</span><span class="p">(</span><span class="n">u</span> <span class="o">+</span> <span class="n">v</span><span class="p">)</span>
            <span class="n">max_c</span> <span class="o">=</span> <span class="nb">max</span><span class="p">([</span><span class="n">math</span><span class="o">.</span><span class="n">fabs</span><span class="p">(</span><span class="n">c</span><span class="p">[</span><span class="mi">0</span><span class="p">]),</span> <span class="n">math</span><span class="o">.</span><span class="n">fabs</span><span class="p">(</span><span class="n">c</span><span class="p">[</span><span class="mi">1</span><span class="p">]),</span> <span class="n">math</span><span class="o">.</span><span class="n">fabs</span><span class="p">(</span><span class="n">c</span><span class="p">[</span><span class="mi">2</span><span class="p">])])</span>
            <span class="k">if</span> <span class="n">max_c</span> <span class="o">==</span> <span class="mi">0</span><span class="p">:</span>
                <span class="c1"># We pick an orthogonal axis, avoiding the smallest one</span>
                <span class="k">if</span> <span class="nb">abs</span><span class="p">(</span><span class="n">u</span><span class="p">[</span><span class="mi">0</span><span class="p">])</span> <span class="o">&gt;</span> <span class="nb">abs</span><span class="p">(</span><span class="n">u</span><span class="p">[</span><span class="mi">1</span><span class="p">]):</span>
                    <span class="k">if</span> <span class="nb">abs</span><span class="p">(</span><span class="n">u</span><span class="p">[</span><span class="mi">1</span><span class="p">])</span> <span class="o">&gt;</span> <span class="nb">abs</span><span class="p">(</span><span class="n">u</span><span class="p">[</span><span class="mi">2</span><span class="p">]):</span>
                        <span class="n">q</span> <span class="o">=</span> <span class="n">Quat</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span> <span class="o">-</span><span class="n">u</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">u</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="mi">0</span><span class="p">)</span>
                        <span class="k">return</span> <span class="n">q</span><span class="o">.</span><span class="n">normalize</span><span class="p">()</span>
                    <span class="k">else</span><span class="p">:</span>
                        <span class="n">q</span> <span class="o">=</span> <span class="n">Quat</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span> <span class="n">u</span><span class="p">[</span><span class="mi">2</span><span class="p">],</span> <span class="mi">0</span><span class="p">,</span> <span class="o">-</span><span class="n">u</span><span class="p">[</span><span class="mi">0</span><span class="p">])</span>
                        <span class="k">return</span> <span class="n">q</span><span class="o">.</span><span class="n">normalize</span><span class="p">()</span>
                <span class="k">elif</span> <span class="nb">abs</span><span class="p">(</span><span class="n">u</span><span class="p">[</span><span class="mi">0</span><span class="p">])</span> <span class="o">&gt;</span> <span class="nb">abs</span><span class="p">(</span><span class="n">u</span><span class="p">[</span><span class="mi">2</span><span class="p">]):</span>
                    <span class="n">q</span> <span class="o">=</span> <span class="n">Quat</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span> <span class="o">-</span><span class="n">u</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">u</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="mi">0</span><span class="p">)</span>
                    <span class="k">return</span> <span class="n">q</span><span class="o">.</span><span class="n">normalize</span><span class="p">()</span>
                <span class="k">else</span><span class="p">:</span>
                    <span class="n">q</span> <span class="o">=</span> <span class="n">Quat</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="o">-</span><span class="n">u</span><span class="p">[</span><span class="mi">2</span><span class="p">],</span> <span class="n">u</span><span class="p">[</span><span class="mi">1</span><span class="p">])</span>
                    <span class="k">return</span> <span class="n">q</span><span class="o">.</span><span class="n">normalize</span><span class="p">()</span>
            <span class="n">c_scl</span> <span class="o">=</span> <span class="p">(</span><span class="mi">1</span> <span class="o">/</span> <span class="n">max_c</span><span class="p">)</span> <span class="o">*</span> <span class="n">c</span>
            <span class="n">norm2_c_scl</span> <span class="o">=</span> <span class="n">c_scl</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">c_scl</span><span class="p">)</span>
            <span class="n">tmp</span> <span class="o">=</span> <span class="p">(</span><span class="n">norm_u_norm_v</span> <span class="o">-</span> <span class="n">u_dot_v</span><span class="p">)</span> <span class="o">*</span> <span class="n">c_scl</span>
            <span class="n">q</span> <span class="o">=</span> <span class="n">Quat</span><span class="p">(</span><span class="n">norm2_c_scl</span> <span class="o">*</span> <span class="n">max_c</span><span class="p">,</span> <span class="n">tmp</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">tmp</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">tmp</span><span class="p">[</span><span class="mi">2</span><span class="p">])</span>
            <span class="k">return</span> <span class="n">q</span><span class="o">.</span><span class="n">normalize</span><span class="p">()</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="n">c</span> <span class="o">=</span> <span class="n">u</span><span class="o">.</span><span class="n">cross</span><span class="p">(</span><span class="n">v</span><span class="p">)</span>
            <span class="n">q</span> <span class="o">=</span> <span class="n">Quat</span><span class="p">(</span><span class="n">norm_u_norm_v</span> <span class="o">+</span> <span class="n">u_dot_v</span><span class="p">,</span> <span class="n">c</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">c</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">c</span><span class="p">[</span><span class="mi">2</span><span class="p">])</span>
            <span class="k">return</span> <span class="n">q</span><span class="o">.</span><span class="n">normalize</span><span class="p">()</span></div>

<div class="viewcode-block" id="Quat.conj"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.Quat.conj">[docs]</a>    <span class="k">def</span> <span class="nf">conj</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
        <span class="k">return</span> <span class="n">Quat</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">w</span><span class="p">,</span> <span class="o">-</span><span class="bp">self</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="o">-</span><span class="bp">self</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="o">-</span><span class="bp">self</span><span class="o">.</span><span class="n">z</span><span class="p">)</span></div></div>


<div class="viewcode-block" id="pose_to_xyz_yaw"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.pose_to_xyz_yaw">[docs]</a><span class="k">def</span> <span class="nf">pose_to_xyz_yaw</span><span class="p">(</span><span class="n">A_tform_B</span><span class="p">):</span>
<span class="w">    </span><span class="sd">&quot;&quot;&quot;Gets the x,y,z yaw of B in A from the SE3Pose protobuf message.&quot;&quot;&quot;</span>
    <span class="n">yaw</span> <span class="o">=</span> <span class="n">Quat</span><span class="o">.</span><span class="n">from_proto</span><span class="p">(</span><span class="n">A_tform_B</span><span class="o">.</span><span class="n">rotation</span><span class="p">)</span><span class="o">.</span><span class="n">to_yaw</span><span class="p">()</span>
    <span class="n">x</span> <span class="o">=</span> <span class="n">A_tform_B</span><span class="o">.</span><span class="n">position</span><span class="o">.</span><span class="n">x</span>
    <span class="n">y</span> <span class="o">=</span> <span class="n">A_tform_B</span><span class="o">.</span><span class="n">position</span><span class="o">.</span><span class="n">y</span>
    <span class="n">z</span> <span class="o">=</span> <span class="n">A_tform_B</span><span class="o">.</span><span class="n">position</span><span class="o">.</span><span class="n">z</span>
    <span class="k">return</span> <span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="n">z</span><span class="p">,</span> <span class="n">yaw</span><span class="p">)</span></div>


<div class="viewcode-block" id="is_within_threshold"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.is_within_threshold">[docs]</a><span class="k">def</span> <span class="nf">is_within_threshold</span><span class="p">(</span><span class="n">pose_3d</span><span class="p">,</span> <span class="n">max_translation_meters</span><span class="p">,</span> <span class="n">max_yaw_degrees</span><span class="p">):</span>
<span class="w">    </span><span class="sd">&quot;&quot;&quot;Determines whether the given SE3 pose is small enough in X, Y, and theta.&quot;&quot;&quot;</span>
    <span class="n">delta</span> <span class="o">=</span> <span class="n">SE2Pose</span><span class="o">.</span><span class="n">flatten</span><span class="p">(</span><span class="n">SE3Pose</span><span class="o">.</span><span class="n">from_proto</span><span class="p">(</span><span class="n">pose_3d</span><span class="p">))</span>
    <span class="n">dist_2d</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">sqrt</span><span class="p">(</span><span class="n">delta</span><span class="o">.</span><span class="n">x</span> <span class="o">*</span> <span class="n">delta</span><span class="o">.</span><span class="n">x</span> <span class="o">+</span> <span class="n">delta</span><span class="o">.</span><span class="n">y</span> <span class="o">*</span> <span class="n">delta</span><span class="o">.</span><span class="n">y</span><span class="p">)</span>
    <span class="n">angle_deg</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">degrees</span><span class="p">(</span><span class="nb">abs</span><span class="p">(</span><span class="n">delta</span><span class="o">.</span><span class="n">angle</span><span class="p">))</span>
    <span class="k">return</span> <span class="p">(</span><span class="n">dist_2d</span> <span class="o">&lt;</span> <span class="n">max_translation_meters</span><span class="p">)</span> <span class="ow">and</span> <span class="p">(</span><span class="n">angle_deg</span> <span class="o">&lt;</span> <span class="n">max_yaw_degrees</span><span class="p">)</span></div>


<div class="viewcode-block" id="recenter_angle"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.recenter_angle">[docs]</a><span class="nd">@deprecated</span><span class="p">(</span><span class="n">reason</span><span class="o">=</span><span class="s1">&#39;Use recenter_angle_mod or recenter_value_mod instead.&#39;</span><span class="p">,</span> <span class="n">version</span><span class="o">=</span><span class="s1">&#39;3.2.0&#39;</span><span class="p">)</span>
<span class="k">def</span> <span class="nf">recenter_angle</span><span class="p">(</span><span class="n">q</span><span class="p">,</span> <span class="n">lower_limit</span><span class="p">,</span> <span class="n">upper_limit</span><span class="p">):</span>
    <span class="n">recenter_range</span> <span class="o">=</span> <span class="n">upper_limit</span> <span class="o">-</span> <span class="n">lower_limit</span>
    <span class="k">while</span> <span class="n">q</span> <span class="o">&gt;=</span> <span class="n">upper_limit</span><span class="p">:</span>
        <span class="n">q</span> <span class="o">-=</span> <span class="n">recenter_range</span>
    <span class="k">while</span> <span class="n">q</span> <span class="o">&lt;</span> <span class="n">lower_limit</span><span class="p">:</span>
        <span class="n">q</span> <span class="o">+=</span> <span class="n">recenter_range</span>
    <span class="k">return</span> <span class="n">q</span></div>


<div class="viewcode-block" id="skew_matrix_3d"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.skew_matrix_3d">[docs]</a><span class="k">def</span> <span class="nf">skew_matrix_3d</span><span class="p">(</span><span class="n">vec3_proto</span><span class="p">):</span>
<span class="w">    </span><span class="sd">&quot;&quot;&quot;Creates a 3x3 numpy matrix representing the skew symmetric matrix for a vec3.</span>
<span class="sd">       These are used to create the adjoint matrices for SE3Pose&#39;s, among other things.&quot;&quot;&quot;</span>
    <span class="k">return</span> <span class="n">numpy</span><span class="o">.</span><span class="n">array</span><span class="p">([[</span><span class="mi">0</span><span class="p">,</span> <span class="o">-</span><span class="n">vec3_proto</span><span class="o">.</span><span class="n">z</span><span class="p">,</span> <span class="n">vec3_proto</span><span class="o">.</span><span class="n">y</span><span class="p">],</span> <span class="p">[</span><span class="n">vec3_proto</span><span class="o">.</span><span class="n">z</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="o">-</span><span class="n">vec3_proto</span><span class="o">.</span><span class="n">x</span><span class="p">],</span>
                        <span class="p">[</span><span class="o">-</span><span class="n">vec3_proto</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="n">vec3_proto</span><span class="o">.</span><span class="n">x</span><span class="p">,</span> <span class="mi">0</span><span class="p">]])</span></div>


<div class="viewcode-block" id="skew_matrix_2d"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.skew_matrix_2d">[docs]</a><span class="k">def</span> <span class="nf">skew_matrix_2d</span><span class="p">(</span><span class="n">vec2_proto</span><span class="p">):</span>
<span class="w">    </span><span class="sd">&quot;&quot;&quot;Creates a 2x1 numpy matrix representing the skew symmetric matrix for a vec2.</span>
<span class="sd">       These are used to create the adjoint matrices for SE2Pose&#39;s, among other things.&quot;&quot;&quot;</span>
    <span class="k">return</span> <span class="n">numpy</span><span class="o">.</span><span class="n">array</span><span class="p">([[</span><span class="n">vec2_proto</span><span class="o">.</span><span class="n">y</span><span class="p">,</span> <span class="o">-</span><span class="n">vec2_proto</span><span class="o">.</span><span class="n">x</span><span class="p">]])</span></div>


<div class="viewcode-block" id="matrix_from_proto"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.matrix_from_proto">[docs]</a><span class="k">def</span> <span class="nf">matrix_from_proto</span><span class="p">(</span><span class="n">proto</span><span class="p">):</span>
<span class="w">    </span><span class="sd">&quot;&quot;&quot;Converts a geometry_pb2.Matrix or geometry_pb2.Matrixf to a numpy array.&quot;&quot;&quot;</span>
    <span class="k">return</span> <span class="n">numpy</span><span class="o">.</span><span class="n">array</span><span class="p">(</span><span class="n">proto</span><span class="o">.</span><span class="n">values</span><span class="p">)</span><span class="o">.</span><span class="n">reshape</span><span class="p">(</span><span class="n">proto</span><span class="o">.</span><span class="n">rows</span><span class="p">,</span> <span class="n">proto</span><span class="o">.</span><span class="n">cols</span><span class="p">)</span></div>


<div class="viewcode-block" id="transform_se2velocity"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.transform_se2velocity">[docs]</a><span class="k">def</span> <span class="nf">transform_se2velocity</span><span class="p">(</span><span class="n">a_adjoint_b_matrix</span><span class="p">,</span> <span class="n">se2_velocity_in_b</span><span class="p">):</span>
<span class="w">    </span><span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Changes the frame that the SE(2) Velocity is expressed in. More specifically, it converts the</span>
<span class="sd">    SE(2) Velocity in frame b to a SE(2) Velocity in frame c using the adjoint matrix a_adjoint_b.</span>

<span class="sd">    Inputs:</span>
<span class="sd">        a_adjoint_b_matrix (3x3 numpy matrix)</span>
<span class="sd">        se2_velocity_in_b (geometry_pb2.SE2Velocity OR math_helpers.SE2Velocity) described in frame B.</span>

<span class="sd">    Returns:</span>
<span class="sd">        math_helpers.SE2Velocity described in frame a. None if the input velocity is an unknown type.</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="k">if</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">se2_velocity_in_b</span><span class="p">,</span> <span class="n">geometry_pb2</span><span class="o">.</span><span class="n">SE2Velocity</span><span class="p">):</span>
        <span class="n">se2_velocity_in_b_vector</span> <span class="o">=</span> <span class="p">(</span><span class="n">SE2Velocity</span><span class="o">.</span><span class="n">from_proto</span><span class="p">(</span><span class="n">se2_velocity_in_b</span><span class="p">))</span><span class="o">.</span><span class="n">to_vector</span><span class="p">()</span>
    <span class="k">elif</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">se2_velocity_in_b</span><span class="p">,</span> <span class="n">SE2Velocity</span><span class="p">):</span>
        <span class="n">se2_velocity_in_b_vector</span> <span class="o">=</span> <span class="n">se2_velocity_in_b</span><span class="o">.</span><span class="n">to_vector</span><span class="p">()</span>
    <span class="k">else</span><span class="p">:</span>
        <span class="k">return</span> <span class="kc">None</span>
    <span class="n">se2_velocity_in_a_vector</span> <span class="o">=</span> <span class="n">numpy</span><span class="o">.</span><span class="n">asarray</span><span class="p">(</span>
        <span class="n">numpy</span><span class="o">.</span><span class="n">matmul</span><span class="p">(</span><span class="n">a_adjoint_b_matrix</span><span class="p">,</span> <span class="n">se2_velocity_in_b_vector</span><span class="p">))</span>
    <span class="n">se2_velocity_in_a</span> <span class="o">=</span> <span class="n">SE2Velocity</span><span class="o">.</span><span class="n">from_vector</span><span class="p">(</span><span class="n">se2_velocity_in_a_vector</span><span class="p">)</span>
    <span class="k">return</span> <span class="n">se2_velocity_in_a</span></div>


<div class="viewcode-block" id="transform_se3velocity"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.transform_se3velocity">[docs]</a><span class="k">def</span> <span class="nf">transform_se3velocity</span><span class="p">(</span><span class="n">a_adjoint_b_matrix</span><span class="p">,</span> <span class="n">se3_velocity_in_b</span><span class="p">):</span>
<span class="w">    </span><span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Changes the frame that the SE(3) Velocity is expressed in. More specifically, it converts the</span>
<span class="sd">    SE(3) Velocity in frame b to a SE(3) Velocity in frame c using the adjoint matrix a_adjoint_b.</span>

<span class="sd">    Inputs:</span>
<span class="sd">        a_adjoint_b_matrix (6x6 numpy matrix)</span>
<span class="sd">        se3_velocity_in_b (geometry_pb2.SE3Velocity OR math_helpers.SE3Velocity) described in frame B.</span>

<span class="sd">    Returns:</span>
<span class="sd">        math_helpers.SE3Velocity described in frame a. None if the input velocity is an unknown type.</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="k">if</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">se3_velocity_in_b</span><span class="p">,</span> <span class="n">geometry_pb2</span><span class="o">.</span><span class="n">SE3Velocity</span><span class="p">):</span>
        <span class="n">se3_velocity_in_b_vec</span> <span class="o">=</span> <span class="p">(</span><span class="n">SE3Velocity</span><span class="o">.</span><span class="n">from_proto</span><span class="p">(</span><span class="n">se3_velocity_in_b</span><span class="p">))</span><span class="o">.</span><span class="n">to_vector</span><span class="p">()</span>
    <span class="k">elif</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">se3_velocity_in_b</span><span class="p">,</span> <span class="n">SE3Velocity</span><span class="p">):</span>
        <span class="n">se3_velocity_in_b_vec</span> <span class="o">=</span> <span class="n">se3_velocity_in_b</span><span class="o">.</span><span class="n">to_vector</span><span class="p">()</span>
    <span class="k">else</span><span class="p">:</span>
        <span class="k">return</span> <span class="kc">None</span>
    <span class="n">se3_velocity_in_a_vec</span> <span class="o">=</span> <span class="n">numpy</span><span class="o">.</span><span class="n">asarray</span><span class="p">(</span><span class="n">numpy</span><span class="o">.</span><span class="n">matmul</span><span class="p">(</span><span class="n">a_adjoint_b_matrix</span><span class="p">,</span> <span class="n">se3_velocity_in_b_vec</span><span class="p">))</span>
    <span class="n">se3_velocity_in_a</span> <span class="o">=</span> <span class="n">SE3Velocity</span><span class="o">.</span><span class="n">from_vector</span><span class="p">(</span><span class="n">se3_velocity_in_a_vec</span><span class="p">)</span>
    <span class="k">return</span> <span class="n">se3_velocity_in_a</span></div>


<div class="viewcode-block" id="quat_to_eulerZYX"><a class="viewcode-back" href="../../../python/bosdyn-client/src/bosdyn/client/math_helpers.html#bosdyn.client.math_helpers.quat_to_eulerZYX">[docs]</a><span class="k">def</span> <span class="nf">quat_to_eulerZYX</span><span class="p">(</span><span class="n">q</span><span class="p">):</span>
<span class="w">    </span><span class="sd">&quot;&quot;&quot;Convert a Quat object into Euler yaw, pitch, roll angles (radians).&quot;&quot;&quot;</span>
    <span class="n">pitch</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">asin</span><span class="p">(</span><span class="o">-</span><span class="mi">2</span> <span class="o">*</span> <span class="p">(</span><span class="n">q</span><span class="o">.</span><span class="n">x</span> <span class="o">*</span> <span class="n">q</span><span class="o">.</span><span class="n">z</span> <span class="o">-</span> <span class="n">q</span><span class="o">.</span><span class="n">w</span> <span class="o">*</span> <span class="n">q</span><span class="o">.</span><span class="n">y</span><span class="p">))</span>
    <span class="k">if</span> <span class="n">pitch</span> <span class="o">&gt;</span> <span class="mf">0.9999</span><span class="p">:</span>
        <span class="n">yaw</span> <span class="o">=</span> <span class="mi">2</span> <span class="o">*</span> <span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="n">q</span><span class="o">.</span><span class="n">z</span><span class="p">,</span> <span class="n">q</span><span class="o">.</span><span class="n">w</span><span class="p">)</span>
        <span class="n">pitch</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">pi</span> <span class="o">/</span> <span class="mi">2</span>
        <span class="n">roll</span> <span class="o">=</span> <span class="mi">0</span>
    <span class="k">elif</span> <span class="n">pitch</span> <span class="o">&lt;</span> <span class="o">-</span><span class="mf">0.9999</span><span class="p">:</span>
        <span class="n">yaw</span> <span class="o">=</span> <span class="mi">2</span> <span class="o">*</span> <span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="n">q</span><span class="o">.</span><span class="n">z</span><span class="p">,</span> <span class="n">q</span><span class="o">.</span><span class="n">w</span><span class="p">)</span>
        <span class="n">pitch</span> <span class="o">=</span> <span class="o">-</span><span class="n">math</span><span class="o">.</span><span class="n">pi</span> <span class="o">/</span> <span class="mi">2</span>
        <span class="n">roll</span> <span class="o">=</span> <span class="mi">0</span>
    <span class="k">else</span><span class="p">:</span>
        <span class="n">yaw</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="mi">2</span> <span class="o">*</span> <span class="p">(</span><span class="n">q</span><span class="o">.</span><span class="n">x</span> <span class="o">*</span> <span class="n">q</span><span class="o">.</span><span class="n">y</span> <span class="o">+</span> <span class="n">q</span><span class="o">.</span><span class="n">w</span> <span class="o">*</span> <span class="n">q</span><span class="o">.</span><span class="n">z</span><span class="p">),</span> <span class="n">q</span><span class="o">.</span><span class="n">w</span> <span class="o">*</span> <span class="n">q</span><span class="o">.</span><span class="n">w</span> <span class="o">+</span> <span class="n">q</span><span class="o">.</span><span class="n">x</span> <span class="o">*</span> <span class="n">q</span><span class="o">.</span><span class="n">x</span> <span class="o">-</span> <span class="n">q</span><span class="o">.</span><span class="n">y</span> <span class="o">*</span> <span class="n">q</span><span class="o">.</span><span class="n">y</span> <span class="o">-</span> <span class="n">q</span><span class="o">.</span><span class="n">z</span> <span class="o">*</span> <span class="n">q</span><span class="o">.</span><span class="n">z</span><span class="p">)</span>
        <span class="n">roll</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="mi">2</span> <span class="o">*</span> <span class="p">(</span><span class="n">q</span><span class="o">.</span><span class="n">y</span> <span class="o">*</span> <span class="n">q</span><span class="o">.</span><span class="n">z</span> <span class="o">+</span> <span class="n">q</span><span class="o">.</span><span class="n">w</span> <span class="o">*</span> <span class="n">q</span><span class="o">.</span><span class="n">x</span><span class="p">),</span>
                          <span class="n">q</span><span class="o">.</span><span class="n">w</span> <span class="o">*</span> <span class="n">q</span><span class="o">.</span><span class="n">w</span> <span class="o">-</span> <span class="n">q</span><span class="o">.</span><span class="n">x</span> <span class="o">*</span> <span class="n">q</span><span class="o">.</span><span class="n">x</span> <span class="o">-</span> <span class="n">q</span><span class="o">.</span><span class="n">y</span> <span class="o">*</span> <span class="n">q</span><span class="o">.</span><span class="n">y</span> <span class="o">+</span> <span class="n">q</span><span class="o">.</span><span class="n">z</span> <span class="o">*</span> <span class="n">q</span><span class="o">.</span><span class="n">z</span><span class="p">)</span>
    <span class="k">return</span> <span class="n">yaw</span><span class="p">,</span> <span class="n">pitch</span><span class="p">,</span> <span class="n">roll</span></div>
</pre></div>

           </div>
          </div>
          <footer>
  

  <hr/>

  <div role="contentinfo">
    <p>
      
      &copy; Copyright 2025 Boston Dynamics. All rights reserved.
      <a href="https://www.bostondynamics.com/privacy-policy">Privacy Policy</a> | 
      <a href="https://www.bostondynamics.com/terms">Terms of Use</a>

    </p>
  </div> 

</footer>
        </div>
      </div>
    </section>
  </div>
  <script>
      jQuery(function () {
          SphinxRtdTheme.Navigation.enable(false);
      });
  </script>
    <!-- Theme Analytics -->
    <script async src="https://www.googletagmanager.com/gtag/js?id=UA-XXXXXXX-1"></script>
    <script>
      window.dataLayer = window.dataLayer || [];
      function gtag(){dataLayer.push(arguments);}
      gtag('js', new Date());

      gtag('config', 'UA-XXXXXXX-1', {
          'anonymize_ip': false,
      });
    </script> 

</body>
</html>